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ABSTRACT 

Extended  Kalman  filtering  is  applied  as  an  extension  of  the  Position  Location  Re- 
porting System  (PLRS)  to  track  a  moving  target  in  the  XY  plane.  The  application  uses 
four  sets  of  observables  which  correspond  to  inputs  from  a  fused-sensor  array  where  the 
sensors  employed  are  acoustic,  seismic,  or  radar.  The  nonlinearities  to  the  Kalman  filter 
occur  through  the  measured  observables  which  are:  bearings  to  the  target  only,  ranges 
to  the  target  only,  bearings  and  ranges  to  the  target,  and  a  Doppler-shifted  frequency 
accompanied  by  the  bearing  to  that  frequency.  The  observables  are  nonlinear  in  their 
relationships  to  the  Cartesian  coordinate  states  of  the  filter. 

Filter  error  covariances  are  portrayed  as  error  ellipsoids  about  the  latest  target  esti- 
mate made  by  the  filter.  Rotation  of  the  ellipsoids  is  accomplished  to  avoid  the  cross 
correlation  of  the  coordinates. The  ellipsoids  employed  are  one  standard  of  deviation  in 
the  rotated  coordinate  system  and  correspond  to  a  constant  of  probability  of  target  lo- 
cation about  the  latest  Kalman  target  estimate. 

Filtering  techniques  are  evaluated  for  both  stationan.^  and  moving  observers  with 
arbitrarily  moving  targets.  The  objective  of  creating  a  user-friendly,  personal  computer 
based  tracking  akorithm  is  also  discussed. 
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THESIS  DISCLAIMER 

The  reader  is  cautioned  that  the  computer  programs  developed  in  this  research  may 
not  have  been  exercised  for  all  cases  of  interest.  While  ever\'  effort  has  been  made  within 
the  time  available  to  ensure  that  the  programs  are  free  of  computational  or  logical  er- 
rors, they  cannot  be  considered  validated.  Any  application  of  these  programs  without 
additional  verification  is  at  the  risk,  of  the  user. 
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I.     INTRODUCTION 

The  militan-  principle  of  combat  maneuver,  in  its  most  basic  sense,  relies  upon  the 
knowledge  of  the  ground  commander  as  to  his  position  and  orientation  within  the  com- 
bat arena.  The  units  involved  in  a  joint  maneuver  must  coordinate  their  positions  and 
orientations  with  each  other  in  order  to  ensure  maximum  cooperation  and  the  successful 
prosecution  of  the  militar>'  campaign.  Recent  history  dictates  that  the  knowledge  of  a 
tactical  militar}'  unit  position  has  rested  in  the  individual  skill  of  members  of  the  unit 
with  a  map  and  lensatic  compass,  from  which  a  position  could  be  determined.  Orien- 
tation of  the  unit  could  be  controlled  through  the  accurate  association  of  surrounding 
prominent  terrain  features  to  the  map.  Once  location  and  orientation  have  been  estab- 
lished, they  must  be  transmitted  to  higher  headquarters  in  order  to  ensure  the  proper 
adjacent  unit  coordination.  This  knowledge,  along  with  tactical  intelligence  information 
about  the  position  and  orientation  of  local  enemy  forces  allows  the  ground  commander, 
through  his  staff,  to  command  and  coordinate  support  operations.  These  operations 
would  include  indirect  fire  support,  close-air  support,  deep-air  support,  and  logistics  and 
communication  support. 

The  Position  Location  Reporting  System  (PLRS)  was  created  by  the  Hughes  Cor- 
poration of  Los  Angeles  for  the  Marine  Corps  and  Army  to  ensure,  or  at  least  improve, 
command  knowledge  of  the  positions  and  orientations  of  friendly  tactical  elements 
within  the  combat  arena  [Ref  Ij.  This  system,  however,  does  not  track  enemy  forces 
forward  of  the  Forward  Edge  of  the  Battle  Area  (FEBA).  This  can  be  overcome  by  the 
use  of  an  extended  Kalman  filter  algorithm  designed  as  the  tracking  element  with  inputs 
from  a  fused  sensor  array.  The  sensors  employed  would  be  restricted  to  four  different 
types  of  observables:  target  bearings,  target  ranges,  both  target  bearings  and  target 
ranges,  or  a  Doppler-shifted  frequency  signal  accompanied  by  the  bearing  to  that  signal. 
The  Kalman  filter  algorithm  would  take  the  sensor  inputs  and  through  a  conversion 
process,  show  observed  and  estimated  target  tracks  accompanied  by  a  predicted  track 
based  on  the  last  estimated  position  of  the  target.  An  addition  to  the  Kalman  filter  al- 
gorithm could  then  allow  for  the  display  of  ellipsoids  of  constant  probability  (error 
eUipsoids)  about  the  estimated  target  positions  to  show  an  area  in  which  the  target  is 
located  to  a  definite  probabihty. 


The  operation  of  PLRS  will  be  discussed,  at  least  partially,  in  the  next  chapter.  The 
accumulated  knowledge  of  the  PLRS  observables  (position  and  location  of  friendly 
units)  and  the  observables  of  the  algorithms  developed  in  this  thesis  could  contribute 
greatly  to  the  knowledge  that  a  potential  ground  tactical  commander  could  gain  about 
his  situation  within  the  combat  arena  and  serve  to  aid  in  the  tactical  decision  making 
process.  The  knowledge  of  friendly  and  enemy  forces  within  a  closed  combat  arena  is 
essential  to  the  successful  prosecution  of  military  objectives.  This  axiom  is  best  summed 
up  by  the  Chinese  philosopher  general.  Sun  Tzu  [Ref  2]  who  said 

If  vou  know  the  enemv  and  know  vourself,  vou  need  not  fear  the  result  of  a  hundred 
battles.  If  you  know  yourself  but  not  the  enemy,  for  every  victor}"  gained  you  will 
also  suffer  a  defeat.  If  you  know  neither  the  enemy  nor  yourself,  you  will  succumb 
in  ever>'  battle  .... 

One  can  easily  see  the  necessity  for  accumulated  combat  maneuver  intelligence. 
Since  PLRS  has  no  ability  to  track  an  enemy  force  which  is  forward  of  the  FEBA  and 
concentrates  solely  on  friendly  units,  a  potential  tracking  algorithm  is  developed  in  an 
attempt  to  augment  the  PLRS  system.  The  algorithm  is  as  previously  described  and 
based  on  multiple  fused  sensors  providing  measured  observables  to  the  tracking  element 
for  processing.  Four  types  of  observables  were  considered:  target  bearings  only,  target 
ranges  only,  both  target  bearings  and  target  ranges,  and  a  Doppler-shifted  frequency 
accompanied  by  the  bearing  to  that  frequency.  These  observables  are  then  fed  to  an  ex- 
tended Kalman  filter  simulation  algorithm  to  produce  the  estimated  and  the  predicted 
tracks. 

The  extended  Kalman  filter  assumes  the  existence  of  nonlinearities  between  the  ob- 
servables and  the  system  states.  The  simulations  conducted  restrict  movement  of  the 
target  to  the  XY  plane  and  the  system  states  are  derived  from  these  dimensions.  Non- 
linearities  to  the  filter  algorithms  occur  in  all  of  the  observables  considered  relative  to 
the  derived  system  states.  An  error  ellipsoid  algorithm  is  then  employed  in  order  to  fur- 
ther isolate  the  target  and  to  attempt  to  confirm  its  probable  location.  The  algorithms 
are  then  combined  using  a  personal  computer-based,  user-friendly  driver  routine  in  order 
to  acquire  the  necessary  inputs.  The  total  simulation  program  is  then  analyzed  for  per- 
formance and  an  assessment  of  its  tracking  capabilities  provided. 


II.     POSITION  LOCATION  REPORTING  SYSTEM 

The  PLRS  system  was  developed  for  many  different  reasons  --  among  them  was  the 
ability  to  protect  friendly  tactical  elements  from  fire  from  friendly  sources  during  combat 
situations  and  during  peacetime  training  exercises.  Accidents  occur  every  year  at  the 
Marine  Corps  Air  Ground  Combat  Center  (MCAGCC),  Twentynine  Palms,  California, 
which  are  a  direct  result  of  a  lack  of  knowledge  as  to  subordinate  unit  positions  by 
command  elements  of  friendly  units.  This  is  caused  by  untimely  updating  over  command 
communication  nets.  The  Combined  Arms  Exercise  (CAX)  and  the  Regimental  Firing 
Exercise  (FEX)  are  commonly  used  to  combine  actual  movement  of  troops  with  live  fire 
in  order  to  impress  upon  commanders  the  importance  of  proper  coordination  and  to 
increase  realism  in  combat  training.  All  of  the  movement  corridors  at  MCAGCC  are 
considered  as  live  fire  areas  and,  therefore,  the  positions  of  all  maneuver  elements  within 
them  is  considered  essential  to  both  personnel  safety  and  to  mission  accomplishment. 

The  system  consists  of  a  containerized  master  unit  composed  of  generalized  com- 
puter, communications,  and  command  electronics.  This  unit  is  designated  as  the  Master 
Unit  (MU).  It  is  responsible  for  processing  all  information  inputs  into  the  system  and 
for  the  display  of  unit  positions,  coordination  measures,  and  control  information  visu- 
ally. Locally-placed  user  units  are  man  or  machine  portable  and  are  designated  as  User 
Units  (UU).  The  MU  sends  a  radio  message  to  the  UU  which  then  computes  a  time  of 
arrival  (TOA)  for  the  message  and  transmits  this  information  back  to  the  MU.  The  MU 
then  uses  the  arrival  time  information  from  the  UU  to  compute  a  range  to  the  UU.  The 
UU  also  transmits  the  local  barometric  pressure  to  the  MU.  The  MU  then  uses  the 
range,  bearing,  and  pressure  information  to  compute  a  three-dimensional  position  of  the 
UU.  Since  the  MU  will  not  always  have  linc-of-sight  radio  contact  with  all  of  the  local 
user  units,  the  system  relies  on  the  user  units  to  determine  the  distances  between  them- 
selves and  other  locally-placed  user  units  by  computation  of  a  TOA  for  the  transmissions 
of  the  other  units  and  to  report  this  range  information  to  the  MU.  Reports  are  made 
to  the  MU  through  a  multi-level  relay  technique  using  other  user  units  as  necessary. 

Within  PLRS,  the  location  of  each  unit  is  established  through  a  process  called 
trilateration.  This  involves  taking  the  ranges  from  three  other  units  with  previously  es- 
tabhshed  positions  and  using  those  ranges  to  calculate  the  position  of  the  unlocated  unit. 
To  accomplish  this  the  MU  uses  four  simplified  discrete  Kalman  filters.  These  filters  are 


called  the  Central  Logic  Oscillator  Control  (CLOC)  filter  to  process  clock  offset  and  drift 
rate,  the  Mean  Sea  Level  (MSL)  filter  to  process  an  offset  calibration  for  the  barometric 
pressure  measurements  reported  to  it,  the  Track  Review  and  Correction  Estimation 
(TRACE)  filter  to  enter  and  partially  update  each  UU  position  and  velocity  estimate, 
and  an  altitude  filter  to  process  the  barometric  pressure  data  in  order  to  establish  and 
aid  in  tracking  the  vertical  positions  of  the  local  user  units. 

Tracking  of  the  user  units  is  accomplished  by  the  MU  using  an  internal  coordinate 
system.  The  internal  coordinate  system  serves  as  the  basis  for  all  coordinate  transfor- 
mation processing.  This  system  is  a  transverse  mercator  projection  with  its  central 
meridian  at  the  longitude  of  system  center.  The  system  essentially  takes  the  curved  sur- 
face of  the  earth  and  projects  it  stereographically  onto  a  fiat  plane.  This  is  then  called 
the  Local  Transverse  Mercator  (LTM)  system.  The  LTM  system  produces  a  slight 
amount  of  distortion  for  long  range  measurements  at  distances  far  from  system  center. 
[Ref  3] 

To  use  the  LTM  system,  the  TOA  measurements  must  be  converted  to  LTM  ranges 
by  PLRS  in  real  time.  This  conversion  is  accomplished  in  four  steps  by  the  MU  and  is 
illustrated  in  Figure  1  on  page  5.  The  four  steps  are  listed  as  follows: 

1.  The  conversion  of  the  TOA  measurement  to  a  true  range  measurement. 

2.  The  conceptual  movement  of  the  units  to  an  average  altitude  of  zero. 

3.  The  projection  of  the  earth  onto  a  fiat  plane. 

4.  The  movement  of  the  units  back  to  their  original  altitudes. 

The  use  of  PLRS  greatly  improves  command  and  control  efforts  at  the 
regimental/brigade  level  and  above.  By  provision  of  improved  manuever  unit  movement 
updates  and  an  increase  in  communications  speed,  the  PLRS  concept  significantly  im- 
proves fire  support  coordination  and  logistical  support  efforts.  The  improvements  gained 
decrease  personnel  safety  risks  while  allowing  for  realism  in  peacetime  training,  and  in- 
creased unit  effectiveness  in  combat. 
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Figure  1.      MU  Conversion  of  TOA  Data  to  LTM  Range:     From  (Ref.  3] 


III.     MULTIPLE  GEOMETRY  TARGET  TRACKING  ALGORITHM 

In  order  to  study  the  problem  by  simulation,  three  different  types  of  simulation 
models  had  to  be  developed.  The  sensors,  when  actually  employed  in  the  field,  would 
track  a  real  moving  target  across  the  ground.  Since  a  real  target  does  not  exist,  for  the 
purposes  of  this  simulation,  a  model  had  to  be  developed.  The  sensor  system  must  also 
be  modeled  so  that  the  observables  developed  from  it  can  be  fed  to  the  extended  Kalman 
filter  algorithm  for  processing.  The  extended  Kalman  filter  must  be  modeled  in  a  com- 
puter algorithm  so  as  to  demonstrate  its  tracking  capability.  The  simulations  must  then 
be  iterated  in  order  to  demonstrate  target  movement  and  subsequent  updating  of  the 
estimated  positions  and  velocities  of  the  modeled  target. 

A.  TARGET  MODEL 

The  motion  of  the  target  is  restricted  to  the  XY  plane.  The  simulation  model  uses 
the  basic  equation  of  motion  from  kinematics 

s  =  Sq  + SqT+  — a^T  (3.1) 

This  relation  is  applied  in  both  the  X  and  Y  directions  and  over  the  total  observation 
time 

■^  row/ ~  ^max  ^  ^  (3--^) 

where  k^,^  is  the  number  of  target  track  positions  desired.  Thus,  a  complete  set  of  target 
positions  over  a  defined  time  period  can  be  defined.  These  positions  can  then  serve  as 
the  basis  for  the  sensor  and  Kalman  tracker  algorithms. 

B.  SYSTEM  MODEL 

The  system  modeled  is  that  of  a  ground  target  moving  in  the  XY  plane.  Certain 
restrictions  were  placed  on  the  problem  in  order  to  simplify  the  mathematics.  These  re- 
strictions were: 

•  the  curvature  of  the  earth  is  neglected, 

•  target  and  sensor  movement  are  restricted  to  a  fiat  plane,  and 

•  target  course  and  speed  inputs  are  constant  (i.e.,  step  inputs). 


The  observed  matrix  developed  from  the  sensor  simulation  algorithm  contains  no  noise, 
so  that  Gaussian  noise  can  be  added  to  the  observations  before  entr\-  into  the  extended 
Kalman  filter  algorithm.  This  supports  reahsm  in  the  simulation  in  that  the  observation 
measurements  would  contain  Gaussian  noise. 

This  is  a  linear,  time-distance  system  that  can  be  described  with  equations  of  motion 
for  constant  acceleration  in  two  dimensions.  The  state  space  equation  is 

where 
Xi,  =  state  vector  (estimation  parameter) 

0^  =  state  transition  matrix  (describes  how  the  dynamic  system  states  are  related) 
Ai  =  system  noise  coefficient  matrix 
a^  =  random  forcing  function. 

When  the  above  conditions  and  Equation  (3.3)  are  incorporated,  and  the  observable 
system  is  target  bearings  only,  target  ranges  only,  or  both  target  bearings  and  target 
ranges,  the  state  vector  is 


^t  = 


(3.4) 


When  the  set  of  observables  is  the  Doppler-shifted  frequency  measurement  and  the  as- 
sociated bearing  to  that  frequency,  then  the  state  vector  is 


£k  = 


X 
X 

y 
y 

/o 


(3.5) 


where  yj  is  the  target  transmission  rest  frequency. 

Wlien  the  following  model  equations  are  used  and  the  aforementioned  problem 
conditions  incorporated  into  them,  the  system  state  equation  can  be  expanded  in  matrix 
form.  The  model  equations  are 


^ife+!  — -^/c  +  •^fc^+    2    '^^k^ 


.  ^2 


yk^\  =yk+ykT+-:raykT^ 


2  "^yk^ 


(3.6) 

(3.7) 

(3.8) 

(3.9) 
(3.10) 


Thus  for  the  systems  where  the  state  vector  is  described  by  Equation  (3.4),  the  system 
state  equation  can  be  expressed  as 


/<:+l 


1  r  0  o' 

X 

0    10    0 

0  0  1  r 

X 

y 

k  + 

0    0    0    1 

_y_ 

■'12 

0 

T 

0 

'«x" 

0 

r^/2 

.^y. 

0 

T 

(3.11) 


For  the  systems  where  the  state  vector  is  defmed  by  Equation  (3.5),  the  system  state 
equation  can  be  expressed  as 


X 

1  r  0  0  0 

X 

X 

0    10    0    0 

X 

y 

k+\  - 

0  0  1  r  0 

y 

y 

0    0    0    10 

y 

/o 

0    0    0    0    1 

/o 

+ 


T  r/2  0      0  0 

0  r  0     0  0 

0  0  r  f'i2  0 

0  0  0     r  0 

0  0  0     0  r 


/3 
/4 


(3.12) 


where  /  through  f^  are  the  random  forcing  functions  included  to  account  for  random 
changes  in  speed,  direction,  or  transmission  frequency  which  can  occur  for  a  moving 
target.  Each  of  these  random  forcing  functions  must  be  considered  independently  in  or- 
der to  ascertain  their  effect.  The  forcing  functions  can  be  expressed  by 


/i  =f\iy0,'  Vv,.  ^) 


(3.13) 


fi  =f2iye,^  y^r  ^) 


(3.14) 


/3  =My9;  y^  ^^ 


(3.15) 


/4  =/4(ye,.  Vv,'  ^) 

/s  =/5(y/„) 


(3.16) 
(3.17) 


where  y^^  (for  heading),  y„^  (for  speed),  and  yy  (for  the  transmission  rest  frequency)  are 
considered  as  random  changes  to  the  target.  These  changes  are  assumed  to  be  inde- 
pendent, zero  mean,  and  piecewise  constant  rates  of  change.  They  have  variances  defined 
as 


(3.18) 
(3.19) 
(3.20) 


The  system  noise  process  for  the  target  tracking  and  prediction  problem  is  a  function 
of  the  system  noise  coefficient  matrix  A^t,  and  the  random  forcing  function  a^  as  defined 
for  each  of  the  state  vectors.  For  the  state  vector  of  Equation  (3.4)  this  is  simply  the 
target  acceleration  vector.  For  the  state  vector  of  Equation  (3.5)  a^  is  defined  by 


«*  = 


fi 

/3 
/4 

/s 


(3.21) 


The  state  space  representation  for  the  Doppler- shifted  frequency  observable  was  devel- 
oped by  Mitschang  [Ref  4]. 

C.     MEASUREMENT  MODEL 

The  problem,  as  studied,  considered  four  types  of  observables:  bearings  to  the  target 
only,  ranges  to  the  target  only,  both  bearings  and  ranges  to  the  target,  and  the 
Doppler-shifted  frequency  accompanied  by  a  bearing  measurement  to  that  frequency. 
With  recall  of  the  state  vectors  as  defined  by  Equations  (3.4)  and  (3.5),  the  measurement 
model  for  this  problem  can  be  developed.  The  measurement  equation  is 


.     Zk  =  Hk£k  +  Oi,  (3.22) 

where 
z^  =  the  set  of  measurements 
Hi  =  the  obsen'ation  matrix  (noiseless) 
£i  —  the  state  vector 
v^  =  associated  measurement  noise. 

1.     Bearings  Only  Problem 

In  this  tracking  problem,  the  measurements  are  lines  of  bearing  as  received  by 
sensors  located  on  separate  prominent  terrain  features.  The  geometry  of  the  problem  is 
shown  in  Figure  2  on  page  11.  The  problem  geometr}'  shows  that  the  relationship  of 
the  measurements  to  the  system  states  is  nonlinear.  For  this  case  the  measurement 
equation  becomes 


^nk  =  tan" 


{x,j^  —  X„/^) 
i^'ik  ~~  y'nk) 


+  0,  (3.23) 


where 
6^^  =  lines  of  bearing  from  sensor  n  at  time  k 
-^r*J"rA  =  t^he  X,Y  coordinates  of  the  target  at  time  k 
^nkSnk  =  the  X.Y  coordinates  of  sensor  n  at  time  k 
Vf,  =  the  bearing  measurement  noise. 

2.     Ranges  Only  Problem 

In  this  tracking  problem,  the  measurements  are  straight-line  distances  to  the 
target  gained  by  ranging  sensors  located  on  separate  prominent  terrain  features.  The 
problem  geometry  is  shown  in  Figure  3  on  page  12.  This  geometrv',  as  in  the  last  case, 
shows  a  nonlinear  relationship  between  the  measurements  and  the  system  states.  The 
measurement  equation  is 


''nk  =  J^ik-^nk    +y[k-ynk     +  ^k  (3-24) 

where  r^/,  =  range  distances  from  sensor  n  at  time  k. 
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Figure  2.      Bearings  Only  Geoinetrj':     Measurement  Inputs  to  the  Kalman  Filter 
are  Restricted  to  Target  Bearings. 
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Figure  3.      Ranges  Only  Geometry:     Measurement  Inputs  to  the  Kalman  Filler  are 
Restricted  to  Target  Ranges. 
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3.     Bearings  and  Ranges  Problem 

The  tracking  problem  now  becomes  a  combination  of  the  first  two  problems. 
The  measurements  are  defined  as  in  Equation  (3.23)  for  target  bearings  and  Equation 
(3.24)  for  target  ranges.  The  geometn,'  for  this  problem  is  shown  in  Figure  4  on  page 
14.   The  observation  matrix  for  this  problem  is  now  defined  as 


Zk  = 


^1 

''2 


(3.25) 


4.     Doppler-Shifted  Frequency  Problem 

In  this  tracking  problem,  the  observables  are  a  Doppler-shifted  frequency  ac- 
companied by  a  bearing  measurement  to  that  frequency.  The  problem  geometn.'  as 
shown  in  Figure  5  on  page  15.  along  with  knowledge  of  the  Doppler  equation  [Ref  5], 
shows  that  the  relationship  between  the  measurements  and  the  system  states  is  again 
nonlinear.  The  measurement  equation  is 


tan 


-if    ■'^ik  ~  ■'^nk    1 
L   y'lk  ~  y'nk    J 


'p^ 


^ik^rk+y,!^':k 


,2,2 
■^ik  "^^ik 


+ 


l'jj 


(3.26) 


where 
v^  =  the  velocity  of  wave  propagation  (speed  of  light) 
Vg^  =  bearing  measurement  noise 
v^  =  frequency  measurement  noise. 

This  measurement  equation  is  based  on  a  single  sensor  which  is  stationary  and  located 
at  the  origin. 

The  observables  in  all  four  measurement  cases  have  been  shown  to  be  nonlinear 
in  their  relationship  to  the  system  states.  Processing  by  the  extended  Kalman  filter  is 
thus  required  for  target  tracking  and  prediction.  The  operation  of  the  extended  Kalman 
filter  will  be  explained  in  the  next  chapter. 
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Figure  4.      Bearings  and  Ranges  Geometry:     Measurement  Inputs  to  the  Kalman 
Filter  are  Restricted  to  Target  Bearings  and  Target  Ranges. 


14 


Target 

(f^. 

I 

i 

/     (  Obserued     Frequency  ) 

• 

i_ 

y 

Formard 
Obseruer 

1 

Figure  5.  Doppler  Frequency  Geometry:  Measurement  Inputs  to  the  Kalman 
Filler  are  Restricted  to  a  Doppler  Shifted  Frequency  and  the  Associated 
Frequency  Bearing. 
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IV.     KALMAN  FILTER  THEORY 

The  process  of  estimation  of  the  state  vector  at  the  current  time,  with  reference  to 
all  previous  measurements,  is  referred  to  as  filtering.  An  optimal  filter  optimizes  a  spe- 
cific performance  measurement  which  is  used  to  approximate  the  quality  of  the  estimate. 
The  Kalman  filter  is  a  linear  optimal  filter  which  minimizes  the  mean  square  estimation 
error  between  the  actual  output  and  the  desired  output.  The  filter,  in  actuality,  is  a  re- 
cursive algorithm  for  the  optimal  processing  of  discrete  measurements  or  observations 
[Ref  6].  The  filter  requires  an  a  priori  knowledge  of  the  state  estimate  and  its  error 
covariance  as  well  as  the  current  observation.  System  equations  for  the  Kalman  filter 
are 

and 

Zk  =  Hi^Xj,  +  vi,  (4.2) 


A.     THE  EXTENDED  KALMAN  FILTER 

From  the  measurement  equations  in  the  last  chapter  for  each  of  the  observable 
cases,  one  can  see  that  there  is  a  nonlinear  relationship  between  the  measurements  used 
for  target  tracking  and  the  system  state  variables.  The  adaptation  of  the  Kalman  filter 
to  a  nonlinear  application  is  termed  as  the  extended  Kalman  filter.  A  general  discussion 
of  the  of  this  type  of  filter  algorithm  follows. 

Given  system  and  measurement  equations  of  the  form 

£k  =Mk^  f<-)  +  S{xk,  k)wk  (4.3) 

Zk  =  h{£k,k)  +  vi,  (4.4) 

where 
/,  ^,  h  are  nonlinear  functions  of  the  state  vector  x  , 
Wf,  is  the  plant  excitation  noise, 
v;  is  the  measurement  noise. 


16 


The  plant  excitation  noise  and  the  measurement  noise  are  assumed  as  uncorrelated,  zero 
mean,  and  white.  That  is 


vk/c  =  ^'(o,  Q), 


(4.5) 


V-;,  =  A-(0,  R), 


(4.6) 


and 


£]>/:,  Wj  ]  =  Qi,5 


kj 


£Iifc,  Vj  ]  =  Rj,^ 


kj 


(4.7) 

(4.S) 


where  S,,.  is  the  Kronecker  delta  function  such  that 


dkj  =  1         {k  =J) 


(4.9) 


^;:;  =  0  {k^j). 


(4.10) 


To  apply  the  linear  filter  Equations  (4.1)  and  (4.2),  an  expansion  of  Equations  (4.3) 
and  (4.4)  is  conducted  about  the  best  estimate  of  the  state  at  the  current  time.  This  ex- 
pansion is  accomplished  with  a  Taylor  series  and  only  the  first  order  terms  are  kept. 
With  the  expansion.  Equation  (4.3)  now  yields 


^k+\  =  ±k~:^k  +  ^k^k 


(4.11) 


where 


^  = 


JL 


iA  =  iA 


(4.12) 


Equation  (4.4)  now  yields 


Ik  =  lik^k  +  ^k 


(4.13) 


where 


if.=  ^ 


dh 


CXi 


ik  =  <^k 


(4.14) 
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The  variable  i^  is  the  estimated  value  of  the  state  after  the  k"'  measurement  and  the 
variable  xL^  is  the  predicted  state  value  before  the  k'''  measurement.  These  can  be  speci- 
fied more  completely  by 

^,=yi4_„A-i).  (4.15) 

The  following  set  of  definitions  can  then  be  used  to  derive  the  linear  Kalman  filter 
equations  used  in  the  simulation.  A  state  error  vector  can  be  defined  as 

2k  =  ^k-^k-  (4-16) 

A  predicted  state  error  vector  can  then  be  defined  by 

2'k  =  tk-Xi,.  (4.17) 

Equations  (4.16)  and  (4.17)  allow  the  definition  of  the  covariance  of  state  error  matrix 
Z.  by 

P,=  El2^2l^  '  (4.  IS) 

and  the  definition  of  the  predicted  covariance  of  state  error  matrix  R\  by 

PLu  =  E\1£[].  (4.19) 

The  state  excitation  matrix  ^^  ,  as  seen  in  Equation  (4.7),  is  defined  by 

Q,  =  /r[A,iv-,iv-;A[].  (4.20) 

The  measurement  noise  covariance  matrix  J2*  ,  as  seen  in  Equation  (4.8),  is  defined  by 

Rk=E[.mk']-  (4.21) 

The  definitions  cited  above  provide  the  basis  for  the  construct  for  the  Kalman  filter  al- 
gorithm specified  by  the  set  of  equations  below  [Ref  6] 

BLk+x=h<Rk^+Qjc  (4-22) 

Qk  =  R' MlEkl' kUl  "r  R,r'  (4.23) 

R,  =  U-^kllk'\ELk  (4.24) 
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xL,=M,_„k)  (4.25) 

z:,  =  h{xL„k)  (4.26) 

£,  =  £Lk  +  ^k[ik-zL,l  (4.27) 

where 
I  =  the  identity  matrix 
G,,  =  the  Kalman  gain  matri.x. 

The  Q  matrix  allows  for  target  maneuvering  and  also  accounts  for  filter  model  in- 
accuracies. This  greatly  decreases  the  discrepancies  which  would  arise  between  the  sys- 
tem characterization,  Equations  (4.1)  and  (4.2),  and  the  true  action  of  the  system 
physically.  As  the  filter  algorithm  reaches  steady-state  conditions,  the  Q  matrix  also 
prevents  the  Kalman  gain  matrix,  Q,,  ,  from  approaching  zero  by  ensuring  an  amount 
of  uncertainty  in  the  predicted  covariance  of  the  state  error  matrix  £1* 

B.     LI.NE.ARIZATION  OF  THE  OBSERVATION  MATRIX 

For  purposes  of  brevity,  the  H  matrix  derivations  will  be  done  simultaneously  for 
the  bearings  only,  ranges  only,  and  the  bearings  and  ranges  observable  cases.  These  are 
slightly  different,  however  they  are  based  upon  the  same  mathematical  premise. 
Equations  (4.4),  (4.13),  and  (4.14)  estabUsh  the  basis  for  the  linearization  of  the  obser- 
vation matrix  H.  These  equations  state  that  the  H  matrix  is  a  Unearization  of  the  non- 
linear function  h  and  is  achieved  by  taking  the  partial  derivative  of  h  with  respect  to  the 
predicted  state. 

1.     Target-Bearing  Measurement  Observables  Only 

The  h  function  using  bearing  measurements  only  can  be  deduced  from  Equation 
(3.23)  as 

Applying  the  above  linearization  method, 


c 


tan  ' 


iy'ik  ~  y'nk) 


c.x 


(4.29) 


^;c 
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where  jr^  can  be  recalled  for  Equation  (3.4).    Simplifying  Equation  (4.24j  yields 


Iik  = 


■/j,  1/2,2/2,3/2,4' 

/?2i  /I22  ^23  ^24 


(4.30) 


where 


h„,  = 


tan 


-1 


(y:k-ynk) 


y'tk  ~  ynk 


'/Jl 


dx 


Ik 


R 


nk 


(4.31) 


^^2  = 


tan 


-1 


^'tk  ~  ynk) 


CX^j, 


(4.32) 


^^3  = 


tan 


cy^k 


Kk 


(4.33) 


tan 


(-V 


rk  ~~  ■'^nk'^ 


^Jtk  ~  ynl, 


'n4 


cytk 


=  0. 


(4.34) 


From  Equations  (4.31)  through  (4.34) 


hu 

yik  ~ynk 
^nk 

/2,2  =  0 

hn  = 

~  i^ik  ~  ^nk) 

^2 

^4  =  0 

(4.35) 
(4.36) 
(4.37) 
(4.38) 
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where  the  range  estimate  squared  Rl^  is 


^nk  —  (■'^!(k,  k-\)~  ^nk)    +  iyi{k,  k-\)~  y'nk)  • 


(4.39) 


The  second  row  of  the  matrix  is  completed  in  a  similar  manner. 
2.     Target-Range  Measurement  Observables  Only 

The  target-range-measurement-only  h  function  can  be  deduced  from  Equation 
(3.24)  as 


h{x,„  k)  =  vV:*  -  Xnk)    +  iyik  -y'nk) 


(4.40) 


Applying  the  linearization  method  again  yields 


m  = 


^\_\  i-^^ik  ~  ^nk)    +  ^'tk  ~ y'nld     J 


^k 


CX 


(4.41) 


■-^k 


where  again,  x,  is  recalled  from  Equation  (3.4).    Simplification  of  Equation  (4.36)  yields 


Rk  = 


^21  ^22^23^^24 


(4.42) 


where 


K^  = 


C [ v'(^f/c  -  Xr^kf  +  i}'ik  - ynk)^  J         y^^  -  y^. 


CX.i, 


Rlk 


(4.43) 


Ki  = 


c\_J{xik  -  -"^nkf  +  iytk-ynkf  J 


=  0 


(4.44) 


K2  = 


<5[ v'(^,jt  -  ^nk?  +  iytk  - y'nkf  ]  -  {x,^  -  X„i,) 


Sy, 


tk 


^nk 


(4.45) 


^«4  = 


\J{x,^  -  x„if  +  (y,i,  -y„if  ] 
^y'tk 


=  0. 


(4.46) 


Thus  from  Equations  (4.43)  through  (4.46) 
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■'^rk  ~  ^nk 


hn 

^2 

hn 

=  0 

hn 

(y\ 

k  ~  ynk) 

^14 

=  0 

(4.47) 
(4.48) 
(4.49) 
(4.50) 


and  as  before,  the  second  row  can  be  computed  in  a  similar  manner. 
3.     Target-Bearing  and  Range  Measurement  Observables 

The  h  functions  for  the  case  where  the  observables  are  both  the  target  bearings 
and  the  target  ranges  are  the  same  as  those  derived  in  both  of  the  previous  cases.  These 
cases,  however,  are  now  combined.  The  corresponding  H  matrix  is 


(4.51) 


where  the  first  and  third  row  of  the  matrix  are  the  first  and  second  rows  of  the  matrix 
defined  in  Equation  (4.30)  replaced  in  a  one-for-one  correspondence.  The  second  and 
fourth  rows  of  the  matrix  are  the  first  and  second  rows  of  the  matrix  defined  in  Equation 
(4.42)  replaced  one-for-one. 

4.     Doppler-Shifted  Frequency  Observable 

The  h  function  for  the  Doppler-shifted  frequency  observable  and  the  associated 
bearing  is  somewhat  more  detailed.  Using  the  state  vector  defined  by  Equation  (3.5)  and 
the  measurements  obtained  from  Equation  (3.26),  one  can  deduce  the  following  shifted 


^11  ^'12^13^14 

II,= 

/221  A22  fh2  ^24 
^31  ^32^33^34 

^^41  ^42  ^43  ^44 

22 


Doppler  h  function 


Kxj,,  k)  = 


A 


tan 


■if  -^--^fe  1 
fo{k)vp 


'P  + 


^ilr^ik  +  y  !!■■}' Ik 


-       ^-'^ik  +  y'lk 


(4.52) 


Application  of  the  linearization  process  yields 


Iik  = 


fh\  fhl  ^h3  ^24  ^'25 


(4.53) 


where  using  the  notation  of  Equation  (4.15) 


/'!!  = 


dO, 


—  V 


>  !k 


'^^-'^ik       -r'?/e+y?;c 


(4.54) 


(4.55) 


A,,= 


^^, 


c>V/c      x' ;„+}''■;,, 


(4.56) 


(4.57) 


(4.58) 


v2 


^21  = 


^y^  ~f'ky'  tkW  !k-^'  [k  ~  ^'  rlS''  ikl 


OXtk 


f'^k^-p{.{^']k+y']kf''^ 


(4.59) 


^22  = 


^fk 


~f'k-^'tk 


dx 


,2 


M2- 


'^    f'ok^'pii^'ik+y^tk) '  ] 


(4.60) 
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,  ^fk  ~f'k  ^'  tk\-^'  tk>''  tk  ~y'  tk^'  lk\  t  ^  t:x\ 

^23  — ~ = ; TIT, (-♦•ol) 

^>>^  f'0k^;l{y:k+y:k?'l 


^fk  -f'ly'tk 

^24  =  -T^  = /       ,   ,.,  (4.62) 

'y^^    f'ok'^piwl+y'J''^ 
sfk    f'k 

^25  =  77"  =  77^  (4.63) 

K/O         /   Qk 

Having  linearized  the  observation  matrices  for  all  of  the  observable  cases,  the  linear 
Kalman  filter  equations  can  be  utilized. 

C.     ACQUIRED  NOISE  PROCESSES  (R  AND  Q  MATRICES) 

Calculation  of  the  covariance  of  state  error  matrix,  P^,  and  the  Kalman  gain  matrix 
G;,  requires  the  specification  of  covariance  matrices  for  the  associated  uncorrelated  noise 
processes  a^  and  v^  as  used  in  this  study.  The  covariance  matrix  for  the  measurement 
noise  process  v^  is  found  in  Equation  (4.8).  R^  is  defined  as  the  state  measurement  noise 
covariance  matrix.  This  matrix  is  based  on  the  accuracy  of  the  sensors  employed  and 
accounts  for  unknown  disturbances  in  the  plant  model. 

The  state  excitation  matrix  Q,,  represents  the  system  noise  process  and  is  a  function 
of  the  system  noise  coefficient  matrix  A^  and  the  random  forcing  function  a,,.  .  Thus 

Q,  =  [A,£,A[]  (4.64) 

where  Q\  ,  from  Equation  (4.20),  is  defined  as 


Qj,  =  ElMk^Il  = 


^{(^Xk^k)         Eli^yk) 


(4.65) 


This  matrix  is  valid  for  the  bearings  and  ranges  observables  and  allows  for  any  random 
target  maneuvers  and  any  inaccuracies  in  the  system  model. 

The  derivation  of  the  Q  matrix  for  the  Doppler  observable  is  slightly  more  complex 
however,  and  is  based  on  the  random  disturbances/  through y,  as  defined  by  Equation 
(3.21).  Recall  that  y^,  y,  ,  and  7^  were  the  random  changes  to  the  target  and  were  as- 
sumed as  independent,  zero  mean,  and  piecewise  continuous.  The  variances  for  these 
chances  can  be  defined  as 
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Vo 


=  ^bU 


(4.66) 
(4.67) 
(4.68) 


.  The  standard  deviations  a,^,  a^ ,  and  <7^  specify  typical  maneuvering  parameters  for  the 
target.  The  Q  matrix  is  found  by  letting 


2        /   •*:   s2    2    ,     .2    2 


(4.69) 


2    2 


2        /  >V   n2    2     ,     • ^    / 


^xy  =  ^;>V 


—  C7. 


(4.70) 


(4.71) 


where  the  states  are  evaluated  at  the  current  state  estimates  x^^  Substitution  of  the  above 
expressions  into  the  Q  matrix  yields 


Qk  = 


[T'llfal  {ri2)a]  {T'j2)a%  {t ll)a% 

q\2         {T')ol    {T'I2)c%    (T'^^' 

qn  ^23       {T'm'al   (7^/2)^; 

^14  ^24  q34  {T\1 

q\5  q25  q35  q45 


xy 
2 

V 


t2    2 


(4.72) 


This  analysis  was  done  by  Mitschang  and  is  found  in  detailed  form  in  Reference  4. 

D.     PROBLEM  PARAMETERS 

The  following  parameters  were  used  in  this  analysis: 
og^  =  6.283  rad/hr 
a,^  =  0.01852  km/hr 
<7|^  =0.01096  (rad/min)' 
a]^  =0.0001852  (km/min^)^ 

For  the  Doppler  observable,  the  parameters  were: 
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a,  =  0.00017452  rad/sec 
a^^  =0.01852  km/sec 
a]^  =0.0005  Hz/ sec. 

With  the  4>  matrix  defined  by  the  system  considered  as  in  Chapter  III,  and  the  Q,  R,  and 
H  matrices  defined  as  above,  the  Kalman  filter  Equations  (4.22)  through  (4.27)  can  be 
employed  and  the  simulations  conducted. 
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V.     ERROR  ELLIPSOIDS 

The  position  of  each  unit  within  the  PLRS  system  is  estimated  with  a  certain  fmite 
degree  of  uncertainty  about  the  estimate.  The  associated  uncertainty  :l  expressed  in  the 
covariance  of  error  matrix  Pj,.  This  represents  the  sigma  squared  error  deviation  about 
the  estimate.  The  position  diagonal  terms,  /*,,  and  Pj^,  represent  the  variances  of  the  es- 
timate in  the  X  and  Y  directions  respectively.  The  off-diagonal  terms  of  each  represent 
covariances,  the  degree  of  coordinate  coupling,  and  the  orientation  of  the  uncertainty  in 
the  XY  plane. 

The  errors  are  normally  distributed  and  there  exists  a  rotated  coordinate  system  in 
which  the  orthogonal  position  components  are  uncorrelated.  This  is  identical  to  taking 
the  exponent  of  the  joint  probability  density  function  and  performing  a  coordinate 
transformation  to  eliminate  the  cross  terms. 

The  exponent  for  Gaussian  random  variables  is 

2  2 

^-2^^-^  +  ^-  (5-1) 

where  r,^  is  the  cross  correlation  of  X  and  Y.  When  this  expression  is  set  equal  to  a 
constant,  the  geometric  interpretation  is  that  of  an  ellipse  which  specifies  a  constant 
probability  of  target  location  about  the  estimate.  The  major  and  minor  axis  of  the  ellipse 
are  oriented  in  the  XY  coordinate  system.  In  this  system  a  cross  correlation  between  X 
and  Y  exists.  To  eliminate  this  cross  correlation,  a  coordinate  transformation  is  apphed. 
The  positional  component  X  is  transformed  to  a  new  component  X'  and  the  positional 
component  Y  is  transformed  to  a  new  component  Y'  by 

x' =  X  cosd +y  s'md  (5.2) 

and 

x' =y  cosd  —  X  s'md  (5.3) 

where 

«  =  |tan-'[i^^],  (5.4) 
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This  specifies  the  variances  in  the  new  coordinate  system  (X'Y')  as 


and 


•  2  2 

2        <^x  +  <7v        cov(.rv) 
^  2  sin  26 


2  ^x-^  ^y  COV(.rv) 

■^  2  sm  20 

The  relation  used  to  plot  the  resulting  ellipsoid,  which  defines  a  constant  probability  of 
target  location  about  the  latest  Kalman  estimate,  results  from  the  transformation.  The 
relation  is  based  on  Equation  (5.1)  and  is 

,2  .,'2 

-V  +  ^=K.  (5.7) 

where  K  defines  the  probability  of  target  location. 

The  error  ellipsoid  routine  employed  in  this  study  is  based  on  the  above  premise  and 
plots  eUipsoids  respresenting  a  constant  probability  of  target  location  about  the  latest 
Kalman  estimate  and  uses  that  estimate  as  the  ellipsoid  center.  The  program  was  written 
by  Captain  Stephen  L.  Spehn  USMC  in  collaboration  with  another  project.  This  pro- 
gram plots  a  single  standard  of  deviation  (in  the  orthogonal  coordinates)  ellipsoid  with 
a  sixty  percent  probabiUty  of  target  location  within  the  ellipsoid.  The  major  and  minor 
axis  of  the  ellipse  may  also  be  plotted  in  terms  of  the  Mahalanobis  distance  or  the  dis- 
tance in  standards  of  deviation  [Ref  7].  The  ellipsoids  are  increased  in  size  by  factors  of 
as  much  as  sixteen  for  ease  of  view. 
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VI.     PROGRAM  RESULTS 

A.     PROGRAM  LOGIC 

The  tracking  problem  solution  integrates  all  four  types  of  geometric  concepts  of 
Chapter  IH.  In  order  to  simulate  the  problem  and  to  attempt  a  solution,  each  of  the 
geometric  concepts  required  an  individual  working  subroutine.  The  subroutines  were 
then  integrated  using  a  menu-driven,  user-interactive  driver  file.  The  driver  file  was  de- 
signed to  acquire  target  position  and  movement  information,  tracking  problem  parame- 
ters, and  to  determine  the  type  of  problem  to  be  solved. 

The  program  presented  herein  is  an  attempt  to  solve  the  geometric  problem  of 
tracking  an  arbitrarily  moving  target  in  the  XY  plane.  It  was  written  in  PC-Matlab  in 
order  to  use  a  personal  computer-based  mathematical  algorithm.  The  system,  when 
augmented  and  improved  with  combinational  logic  and  mapping  data,  could  be  used  by 
a  maneuver  commander  in  the  combat  arena  as  an  additional  tool  in  his  assessment  of 
the  tactical  situation.  The  advantage  that  could  be  gained  in  the  knowledge  of  enemy 
position  and  intent  would  greatly  aid  in  the  tactical  decision-making  process. 

1.  PC-Matlab  Structure 

The  personal  computer-based  Matlab  program  uses  a  unique  program  structure 
that  is  similar  to  Fortran.  The  program,  however,  uses  different  terminology  in  its 
structure  when  employing  driver  routines.  The  dilTerence  in  terminology  is  derived  from 
the  way  the  main  Matlab  driver  programs  are  written.  All  Matlab  execution  files  are 
named  with  a  .m  suffix.  Thus,  all  of  the  file  names  for  this  problem  contain  that  suffix. 
The  driver  file,  marine. m,  calls  the  various  subroutines  as  needed  when  in  operation.  The 
programs  written  for  the  study  of  this  problem  are  included  in  the  Appendix.  They  are 
commented  throughout  in  attempt  to  help  the  reader  to  become  familiar  with  their 
construct  with  as  little  effort  as  possible. 

2.  Subroutine  Structure 

The  program  subroutines  each  isolate  one  geometric  concept  in  an  attempt  to 
simulate  and  solve  the  tracking  problem.  Each  subroutine  is  written  to  simulate  the  tar- 
get, to  simulate  the  observers  (sensors),  and  to  simulate  the  operation  of  the  extended 
Kalman  filter  discussed  in  Chapter  IV.  The  various  steps  are  commented  throughout  in 
an  effort  to  aid  the  reader  in  understanding  the  program  and  its  structure.  The  names 
of  the  variables  used  in  the  programs  of  the  Appendix  correspond  to  those  of  Chapters 
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Ill  and  IV  as  much  as  possible.  It  was  felt  that  this  would  aid  in  the  understanding  of 
its  operation. 

B.     RESULTS 

The  results  of  the  program  discussed  are  presented  graphically  in  order  to  illustrate 
what  the  maneuver  commander  would  see  if  such  a  system  were  to  be  employed  on  the 
battlefield.  Since  the  program  tracks  an  arbitrarily  moving  target  with  arbitrary  observer 
placement,  the  target  tracks  and  the  observer  (sensor)  locations  were  changed  with  each 
geometric  case.  The  cases  will  be  presented  in  the  following  order:  target-bearing  ob- 
servables  only  employed,  target-range  observables  only  employed,  both  target-bearing 
and  range  observables  employed,  and  the  Doppler  observable  with  its  associated  bearing 
employed. 
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1.      Target- Bearing  Observables  Only 

Results  of  a  tracking  problem  using  only  the  observed  bearings  from  a  fused- 
sensor  array  are  contained  in  Figure  6  on  page  32  through  Figure  9  on  page  35.  These 
figures  show  outputs  at  various  stages  of  the  program.  Figure  6  on  page  32  shows  the 
actual  track  of  the  target,  the  circles  on  the  diagram,  and  the  noiseless  observations,  the 
dots.  This  graph  allows  for  a  comparison  of  the  bearing  calculations  by  the  program 
against  the  track  as  chosen  in  the  simulation.  This  is  necessary'  as  a  check  in  the  opera- 
tion of  the  bearings  only  subroutine.  If  the  comparison  is  successful,  white  noise  is  added 
to  the  bearing  measurements  before  they  are  submitted  to  the  Kalman  algorithm. 
Figure  7  on  page  33  shows  the  actual  and  estimated  target  tracks  as  well  as  the  error 
ellipsoids  around  the  Kalman  estimates.  The  simulation  iterates  and.  when  in  display  on 
the  screen,  one  can  see  the  target  move.  In  this  case,  the  movement  is  from  the  upper 
left  to  the  lower  right.  The  rotation  of  the  error  eilipsiods  to  avoid  coordinate  cross 
correlation  is  clearly  evident.  The  actual  positions  are  again  circles,  the  estimates  are 
crosses,  and  the  error  ellipsoids  are  evident  from  the  plot.  Figure  S  on  page  34  shows  the 
actual,  estimated,  and  predicted  positions  of  the  target.  The  predictions  were  obtained 
by  taking  the  predicted  Kalman  state  estimate  and  the  current  Kalman  state  estimate 
and  performing  a  subtraction  of  the  velocity  values  in  the  X  and  Y  directions  and  di- 
viding by  the  observation  interval.  This  produced  an  estimate  of  the  random  forcing 
function  a^  as  defmed  in  Chapter  III.  The  estimate  was  then  used  in  the  linear  Kalman 
system  equation  shown  in  Equation  (4.1)  to  determine  predicted  positions  based  on  a 
constant  acceleration.  This  is  consistent  with  a  second-order  Kalman  analysis  in  each 
direction  where  the  acceleration  is  the  random  forcing  function.  The  predicted  positions 
are  shown  as  stars  in  the  diagram.  Figure  9  on  page  35  shows  what  the  potential  combat 
commander  would  see  should  the  system  be  employed.  The  figure  is  a  combination  of 
the  actual,  estimated,  and  predicted  target  tracks  as  well  as  the  error  ellipsoids  about  the 
Kalman  estimates.  The  figure  also  shows  the  location  of  the  observers,  as  do  the  previ- 
ous figures.  These  are  portrayed  as  an  x  in  the  diagram.  Essentially  the  program  was 
constructed  to  produce  results  in  response  to  questions  a  potential  maneuver 
commander  would  ask  while  engaged  in  combat  operations.  These  questions  are: 

•  Where  is  the  enemy  ? 

•  Where  is  he  going  ? 
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Figure  6.      Actual  and  Observed  Tracks:     Target-Bearing  Observables  Only.    Ac- 
tual Track  -  o,  Observed  Track  -  •  ,  Sensor  -  x. 


32 


Figure  7.  Actual  and  Estimated  Tracks:  Target-Bearing  Observables  Only.  Ac- 
tual Track  -  o,  Estimated  Track  -  +  .  The  Plot  Also  Contains  Error 
Ellipsoids  to  Aid  in  Target  Location. 
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rigure  8.      Actual,  Estimated,  and  Predicted  Tracks:     Target-Bearing  Observables 
Only.  Actual  Track  -  o,  Estimated  Track  -  + ,  Predicted  Track  -  *. 
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Figure  9.      Total  Target  Track  View:     Target-Bearing  Observables  Only.    Actual 
Track  -  o,  Observed  Track  -  • ,  Sensor  -  x.   Predicted  Track  -  *. 
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2.  Target- Range  Observables  Only 

The  results  of  a  tracking  problem  using  only  target-range  observables  are  shown 
in  Figure  10  on  page  37  through  Figure  13  on  page  40.  These  figures  are  in  the  same 
order  as  the  ones  in  the  bearings  only  case.  The  target  track  and  observer  locations  have 
been  changed  in  order  to  demonstrate  the  versatility  of  the  program.  Figure  10  on  page 
37  shows  the  noiseless  observations  compared  to  the  actual  track  of  the  target  as  chosen 
in  the  simulation.  Again,  if  the  comparison  is  successful,  Gaussian  noise  is  added  before 
filtering.  Figure  1 1  on  page  38  shows  the  actual  and  estimated  tracks  of  the  target  as 
well  as  the  error  ellipsoids  about  the  Kalman  estimates.  Figure  12  on  page  39  shows  the 
actual,  estimated  and  predicted  target  tracks.  When  the  display  is  on  screen,  one  can 
observe  the  target  movement,  which  in  this  case  would  be  from  left  to  right  across  the 
diagram.  Since  the  observers  are  nearly  colinear  with  the  first  estimate,  the  error  ellipsoid 
shows  a  substantial  bearing  error  while  showing  a  small  range  error.  This  decreases  as 
the  target  moves  away  from  the  observers  because  the  range  directions  are  more  accu- 
rately determined.  Figure  13  on  page  40  is  a  combination  of  the  previous  figures  which 
would  be  used  to  provide  information  about  the  enemy  for  use  in  the  tactical  decision 
making  process. 

3.  Target-Bearing  and  Range  Observables 

The  results  of  a  problem  employing  both  target-bearing  and  range  observables 
are  shown  in  Figure  14  on  page  41  through  Figure  17  on  page  44.  The  figures  are  again 
in  the  same  order  as  the  previous  two  cases  to  allow  comparison.  Figure  14  on  page  41 
shows  the  actual  target  track  and  the  noiseless  observations  for  this  case.  If  the  obser- 
vations favorably  compare  with  the  actual  positions,  white  noise  is  added  and  the  noisy 
observations  filtered  by  the  Kalman  algorithm.  Figure  15  on  page  42  shows  the  actual 
and  estimated  tracks  with  the  error  ellipsoids  added.  The  rotation  of  the  ellipsoids  to 
avoid  cross  correlation  of  the  coordinate  variables  is  particularly  evident  in  this  case. 
Figure  16  on  page  43  shows  the  actual,  estimated,  and  predicted  tracks  for  the  target. 
The  target  movement  in  this  case  was  from  the  lower  right  to  the  upper  left.  Figure  17 
on  page  44  is  a  combination  of  the  previous  three  and  would  be  the  result  of  the  program 
when  viewed  in  the  field.  Again,  it  provides  answers  to  relevant  command  questions. 
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Figure  10.      Actual  and  Observed  Tracks:     Targel-Range  Observables  Only.    Ac- 
tual Track  -  o,  Observed  Track  -  • ,  Sensor  -  x. 


37 


1 

..« - 

1 

::::•::: 

i        •    i 

1        1        i     ^;        1        1 

M   i  "^   M 

::!♦:; 

::!;:!; 

:         i         I         i         i         i         : 

o 

00 


o 


o 


o 


o 


o 


o 


U 


CO 


o 


o 


O 


« 


o       «o 
fs      1-1 


U-) 


A  HDNVlSia 


Figure  11.  Actual  and  Estimated  Tracks:  Target-Range  Observables  Only.  Ac- 
tual Track  -  o,  Estimated  Track  -  +  .  The  Plot  Also  Contains  Error 
Ellipsoids  to  Aid  in  Target  Location. 
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Figure   12.      Actual,  Estimated,  and  Predicted  Tracks:     Target-Range  Observables 
Only.   Actual  Track  -  o,  Estimated  Track  -  + ,  Predicted  Track  -  *. 
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Figure  13.       Total  Target  Track  View:     Target- Range  Observables  Only.    Actual 
Track  -  o,  Observed  Track  -  •  ,  Sensor  -  x.   Predicted  Track  -  *. 
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Figure  14.      Actual  and  Obsened  Tracks:     Target-Bearing  and  Range  Observables. 
Actual  Track  -  o,  Observed  Track  -  •  ,  Sensor  -  x. 
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Figure  15.  Actual  and  Estimated  Tracks:  Target-Bearing  and  Range  Observables. 
Actual  Track  -  o,  Estimated  Track  -  +  .  The  Plot  Also  Contains  Error 
Ellipsoids  to  Aid  in  Target  Location. 
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Figure  16.      Actual,  Estimated,  and  Predicted  Tracks:     TargetrBearing  and  Range 
Observables.    Actual  Track  -  o,  Estimated  Track  -  + ,  Predicted  Track 
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Figure  17.      Total  Target  Track  View:     Target-Bearing  and  Range  Observables. 
Actual  Track  -  o,  Observed  Track  -  •  ,  Sensor  -  x.    Predicted  Track 
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4.     Doppler- Frequency  Observable 

The  results  of  a  problem  where  a  Doppler-shifted  frequency  and  the  associated 
bearing  to  that  frequency  are  employed  are  shown  in  Figure  18  on  page  46  through 
Figure  21  on  page  49.  These  are  based  on  a  system  employing  one  sensor,  which  is  sta- 
tionary' and  located  at  the  origin.  Figure  18  on  page  46  shows  the  actual  track  positions 
compared  to  the  target  observations.  If  the  observations  compare  to  the  actual  posi- 
tions, white  noise  is  added  and  the  observables  sent  to  the  Kalman  algorithm  for  proc- 
essing. Figure  19  on  page  47  shows  the  actual  and  estimated  tracks  with  the  error 
ellipsoids  to  aid  in  defining  target  position.  Figure  20  on  page  48  shows  the  actual,  es- 
timated ,  and  predicted  target  tracks.  Figure  21  on  page  49  is  again  a  combination  of  the 
previous  three  graphs  and  would  be  the  one  seen  in  the  field.  The  target  movement  in 
this  case  was  from  upper  left  to  lower  right. 
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Figure  18.      Actual  and  Observed  Tracks:     Doppler  Frequency  Observable.  Actual 
Track.  -  o,  Observed  Track  -  •  ,  Sensor  -  x. 
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Figure  19. 


Actual  and  Estimated  Tracks:  Doppler  Frequency  Observable.  Ac- 
tual Track  -  o,  Estimated  Track  -  +  .  The  Plot  Also  Contains  Error 
Ellipsoids  to  Aid  in  Target  Location. 
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Figure  20.       Actual,  Estimated,  and  Predicted  Tracks:     Doppler  Frequency  Ob- 
servable.  Actual  Track  -  o,  Estimated  Track  -  + ,  Predicted  Track  -  *. 
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Figure  21.      Total  Target  Track  View:     Doppler  Frequency  Observable.    Actual 
Track  -  o,  Observed  Track  -  • ,  Sensor  -  x.   Predicted  Track  -  *. 
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VII.     CONCLUSIONS  AND  RECOMMENDATIONS 

A.  CONCLUSIONS 

The  target  tracking  algorithm  developed  here  is  both  personal  computer-based  and 
user-friendly.  This  would  aid  in  field  employment  where  the  system  would  be  of  most 
use  to  the  maneuver  commander.  With  improvements  in  the  tracking  algorithm  pre- 
sented, or  on  one  similiarly  based,  real-time  tracking  could  be  accomplished.  These  im- 
provements will  be  subsequently  discussed.  With  real-time  tracking,  indirect  supporting 
arms  or  direct  air  could  engage  the  target  with  a  reasonable  chance  of  success.  If  the 
orientation  of  the  target  were  known,  optimal  damage  could  then  be  inflicted  upon  it. 
Hence,  the  development  of  a  working  fused-sensor  system  tracking  algorithm  would 
place  forward  reconnaissance  personnel  at  intelligent  risk  when  seeking  combat  maneu- 
ver intelligence  far  forward  of  the  FEBA. 

B.  FUTURE  STUDY  POSSIBILITIES 

Future  study  regarding  improvements  in  the  algorithm  developed  is  quite  extensive. 
Topics  which  would  facilitate  the  program's  usefulness  include  incorporation  of  the  ac- 
celeration terms  in  the  X  and  Y  directions  and  the  application  of  the  Z  coordinate  and 
its  velocity  and  acceleration  terms.  PLRS  uses  time  division  multiple  access  (TDMA)  for 
unit  communications  transmissions.  A  delay  study  should  be  done  so  as  to  account  for 
a  worst-case  analysis  of  the  effect  that  transmission  delays  would  have  on  the  employ- 
ment of  the  Kalman  tracker  or  its  target  update  capabihty. 

C.  PROGRAM  IMPROVEMENTS 

Improved  program  structure  can  be  achieved  by  employment  of  the  principles  used 
by  Baheti  [Ref  8|.  The  Baheti  algorithm  requires  one  quarter  of  the  memor\'  capacity 
and  fewer  computations  than  the  algorithm  as  developed  here,  but  does  not  adhere  to 
conventional  control  methods.  The  algorithm  herein  presented  was  written  in  PC  Matlab 
which  is  a  matrix  manipulation  program.  It  will  require  updating  as  improvements  in  the 
Matlab  algorithm  are  developed. 

Since  optimal  tracking  of  a  ground  target  would  be  futile  in  the  fact  that  the  ground 
target  motion  would  be  unpredictable,  another  possible  program  improvement  could  be 
researched.  The  Defense  Mapping  Agency  has  worldwide  computer  mapping  data  which 
can  be  displayed  on  a  computer  terminal.  It  may  be  possible  to  incorporate  this  data  in 
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a  logic  algorithm  structure  so  as  to  determine  the  target's  tendencies  to  augment  the 
optimal  tracking  scenario.  Once  the  logical  choices  are  made,  the  potential  maneuver 
commander  could  employ  logic  as  well  as  optimal  control  to  thwart  his  adversary". 
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APPENDIX      TARGET  TRACKING  SIMULATION  PROGRAMS 

A.  PROGRAM  DISCUSSION 

The  programs  contained  herein  were  written  to  simulate  the  varipus  aspects  of  the 
target  tracking  problem  heretofore  discussed.  They  are  wTitten  in  PC-Matlab  and  hence, 
are  denoted  by  a  .m  suffix  to  identify  them  as  control  or  execution  files.  A  caveat  should 
be  issued  at  this  point.  This  set  of  programs  was  written  in  PC-Matlab  version  3.5  which 
contains  some  newer  function  files  not  previously  available.  One  should  check  the  ver- 
sion of  PC-Matlab  employed  before  attempting  tracking  simulations.  If  the  function  files 
necessary  to  run  these  programs  are  not  present,  an  error  will  result.  The  programs  are 
commented  throughcut  in  the  hope  that  they  will  aid  the  user  in  understanding  their 
operation. 

The  niain  driver  file  for  the  set  is  titled  marine. m.  The  subroutines  are  titled 
brgbrg.m.  rngrng.m.  brgrng.m.  and  doppler.m.  The  file  errellip.m  is  called  by  each  of  the 
«;ubroutines  as  a  function  file.  The  function  file  reshape. m,  is  also  necessan.'  for  the  op- 
eration of  marine. m.  It  is  contained  for  reference.  These  programs  should  not  be  con- 
sidered as  validated  or  free  of  logical  or  arithmetic  error. 

B.  PROGR.A.M  LISTING 


X  JOHN  A.    HUCKS    II 

%  FILE   NAME:     MARI.NE.  M 

\  CONTROL  FILE   FOR  MULTIPLE   GEOMETRY  EXTENDED  KALMAN  FILTER  TARGET 

%  TRACKING   SIMULATION   PROGRAM. 

%  FILES  NECESSARY  FOR  THE   OPERATION  OF  THIS   PROGRAM  ARE: 

%  BRGBRG.  M 

%  RNGRNG.  M 

%  ERGRNG.  M 

%  DOPPLER. M 

%  ERRELLIP.M  (COURTESY  OF  STEPHEN  L.    SPEHN) 

%  RESHAPE. M  (FUNCTION  FILE) 

%  THIS   PROGRAM  DRIVES   OTHER  MATLAB  M   (EXECUTION)   FILES  WHICH  SIMULATE 

%  TARGET  TRACKI.NG   BASED  ON  VARIOUS   OBSERVABLES,   THESE   BEING  BEARINGS, 

%  RANGES,    BEARINGS   AND   RANGES,    OR  A   DOPPLER   SHIFTED   FREQUENCY.    THE 

%  PROGRAMS   ARE   BASED  ON  FUSED  MULTIPLE   OBSERVERS  THAT  ARE  EITHER 

%  STATIONARY  OR  MOVING. 

%  PROGRAM   SETUP 

clear , ho Id   of f, subplot ( lll),clg,clc 

title  line  =    '  TARGET  TRACKING  SIMULATOR'; 

titlelinel   =    '  (OPTIMAL)  '; 
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titleline2  =  ' 
titlelineS  =  ' 


NAVAL  POSTGRADUATE  SCHOOL  MSEE  THESIS'; 
CAPTAIN  JOHN  A.  HUCKS  II  USMC  DEC.  1989'; 


%  TITLE  DISPLAY 

disp( 

disp( 

dispC 

disp( 

disp( 

disp( 

dispC 

disp( 

disp( 

disp( 

disp( 

disp(titleline); 

disp(titlelinel) 

dispf titleline2) 

disp( titlelineS) 

pause 


%     OBSERVER  INPUT  ROUTINE 


clc 

disp( 
disp( 
disp( 
disp( 
disp( 
disp( 
disp( 
disp( 
dispC 
disp( 
disp( 
disp( 
disp( 


'); 
•); 
•); 

ENTER  THE  FIRST  OBSERVERS  INITIAL  PARAMETERS: '); 


); 
); 


'); 


xfol  =  [ 


X 

% 

vx 

% 

y 

% 

vy  ]; 

0/ 

/o 

observer  x  position  (km)  ') 
velocity  x  direction  (km/hr)') 
observer  y  direction  (km)') 
%  velocity  y  direction  (km/hr)') 


YOU  MAY  ENTER  THIS  IN  THE  FOLLOWING  FORMAT:  [ x  vx  y  vy]  '  '); 
xfols  =  input('xfol  =  ','s'); 
eval(['xfol  =  ', xfols ,';'] ); 
xfol  =  reshape(xfol ,4, 1); 


clc 

disp( 
disp( 
disp( 
disp( 
disp( 

') 

■■\ 

ENT] 

BR 

THE  SECOND  OBS 

disp( 
disp( 
disp( 

'); 
'); 

xfo2  =  [  X 

disp( 

vx 

disp( 

y 

disp( 
disp( 
disp( 

') 

YOU 

vy 
MAY  ENTER  THIS  I 

OBSERVERS  INITIAL  PARAMETERS:'); 


%   observer  x  position  (km)') 
%   velocity  x  direction  (km/hr)') 
%  observer  y  direction  (km)  ') 
;        %  velocity  y  direction  (km/hr)') 

THE  FOLLOWING  FORMAT:  [x  vx  y  vy]  "  ); 
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xfo2s  =  input('xfo2  =  ','s'); 
eval([ 'xfo2  =  ' ,xfo2s,'; '] ); 
xfo2  =  reshape(xfo2,4, 1); 

clc 

dispC  '); 

disp( 'ENTER  OBSERVER  MOVEMENT  SPEED: '); 

disp( I  '); 

disp( '  spd 

disp( '  '); 

dispC  •); 

spds  =  inputC'spd  =  ','s'); 

eval([ ' spd  =  ' ,spds, ' ; '] ); 


%   observer  speed  in  km/hr  ' ) 


clc 

disp( 
dispC 
disp( 
disp( 
disp( 
disp( 
disp( 
disp( 
kmaxs 
eval([ 


•); 

ENTER  DESIRED  NUMBER  OF  OBSERVATIONS:  '); 

•); 

kmax  %   observation  number  ' ) 

'); 
'); 

PLEASE  MAKE  THIS  GREATER  THAN  5  SO  THAT  THE  KALMAN  GAINS') 

CAN  STABILIZE. ' ) 

=  input ('kmax  =  ','s'); 

' kmax  =  ' , kmaxs , ' ; ' ] ) ; 


clc 

disp( '  '); 

disp( 'ENTER  DESIRED  NUMBER  OF  PREDICTIONS: '); 

disp( I  '); 

disp( '  kpred 

disp( I  |); 

disp(  '  '); 

kpreds  =  input( 'kpred  =  ','s'); 

eval([ 'kpred  =  ', kpreds ,';'] ); 


%  prediction  number  ' ) 


clc 

disp( 

disp( 

disp( 

disp( 

disp( 

disp( 

disp( 

disp( 

disp( 

disp( 

dts  = 

eval( 


•); 

ENTER  OBSERVATION  INTERVAL:'); 

•); 

dt 

'); 

THE  TIME  INTERVAL  IS  IN  THE  FORM: ') 

•) 

dt  =  time/60' ) 

'); 

FOR  A  10  MINUTE  INTERVAL  ENTER:   dt  =  10/60') 
input ( ' dt  =  ','s'); 
'dt  =  ', dts, ';']); 


%  observation  interval') 


%     TARGET  SIMULATION  PARAMETERS  INPUT  ROUTINE 


clc 

disp( '  '); 
disp( '  '); 
dispC 


FOR  THE  PURPOSES  OF  SIMULATION,'); 
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disp( 

disp( 

disp( 

disp( 

dispC 

disp( 

disp( 

dispC 

disp( 

disp( 

disp( 

disp( 

dispC 

dispC 

disp( 

disp( 

disp( 

disp( 

tgts  =  input('tgt  = 

eval(c' tgt  =  '  jtgts 

tgt  =  reshape( tgt , 7 


ENTER  THE  TARGETS  MOVEMENT  PARAMETERS: '); 


X 

vx 
ax 

y 

vy 
ay 
fo 


%  target  initial  x  position  (km)') 
%   target  initial  x  velocity  (km/hr)') 
%  target  acceleration  in  x  (kni/hrA2)') 
%   target  initial  y  position  (km)') 
%  target  initial  y  velocity  (km/hr)') 
%  target  acceleration  in  y  (km/hrA2)') 
%  transmitter  rest  frequency  (Hz)') 


YOU  MAY  ENTER  THIS  IN  THE  FOLLOWING  FORMAT:  [ x  vx  ax  y  vy  ay  fo]  "); 

'); 
•); 

*  CAUTION:  VELOCITY  OF  THE  TARGET  MUST  NOT  BE  ZERO.  * 


if  (tgt(2 
clc 
disp( 
disp( 
disp( 
disp( 
disp( 
disp( 
disp( 
disp( 
disp( 
disp( 
disp( 
disp( 
disp( 
disp( 
disp( 
disp( 
[x  vx 
disp( 
disp( 
disp( 
disp( 
disp( 


1)  =  0  6c 


,  s  ); 

,1); 

tgt(5,l) 


') 
') 
') 


0) 


); 
); 


FOR  THE  PURPOSES  OF  SIMULATION,'); 
ENTER  THE  TARGETS  MOVEMENT  PARAMETERS: 


); 


[ 


X 

vx 
ax 

y 

vy 
ay 
fo 


X  position  (km) ' ) 
X  velocity  (km/hr)') 


initial 

initial 

acceleration  in  x  (km/hrA2)') 

initial  y  position  (km)') 

initial  y  velocity  (km/hr)') 

acceleration  in  y  (km/hrA2)') 


end 


target 
target 
target 
target 
target 
target 
transmitter  rest  frequency  (Hz)') 

'); 
'); 

YOU  MAY  ENTER 
ax  y  vy  ay  fo] 

'); 

'); 

mS*  .u  .*•  jL  .L.  ji.  .r.  j;.  ^«  ^.  ^.  J*  .*«  ^.  j^  j~  *3.  ^m  -*fJ^  y-  y*  >y  ^  'Jf  y^  y^  y^  y^  y^  y  ^  y^  ^y  y-  y^  y^  ^  y*  tWt  Vr  t'c  Vr  Vr  ^  Vr  V'  Vr  V*  ?V  Vc  V.**  ^ 

VELOCITY  OF  THE  TARGET  MUST  NOT  BE  ZERO.  * 

tgts  =  input('tgt  =  ','s'); 
eval([ 'tgt  =  ' ,tgts,'; '] ); 
tgt  =  reshape(tgt,7,l); 


THIS 

"); 


IN  THE  FOLLOWING  FORMAT: 


*  CAUTION: 


') 
•) 
') 


global  xfol  xfo2  spd  kmax  kpred  dt  tgt; 

%     OBSERVABLE  MENU  CHOICE  ROUTINE 

choicel  =  -10; 
while  choicel  =  0 
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clc 

disp( I  |); 
disp( I  |); 
disp( '  '); 
disp( title  line); 
disp(  titlelinel); 


disp( 
disp( 
disp( 
disp( 
disp( 
disp( 
disp( 
disp( 
disp( 
disp( 
disp( 
disp( 
disp( 
disp( 
disp( 
dispC 
disp( 
disp( 
disp(. 
disp( 


); 
); 
); 

); 


); 

); 


ENTER  THE  METHOD  OF  OBSERVATION  USED  FOR  THIS  RUN:  '); 

1.  Bearing/Bearing'); 

2.  Bearing/Ranee'); 

3.  Range/Range  ); 

4.  Shifted  Doppler  Frequency'); 

5.  Display  the  last  graph'); 

6.  Change  the  number  of  observations'); 

7.  Change  the  number  of  predictions'); 

8.  Change  the  observation  interval'); 

9.  Change  position  of  first  observer') 

10.  Change  position  of  second  observer 

11.  Change  observer  movement  speed'); 

12.  Change  target  simulation  parameter 


•); 
s'); 


choicel  =  input( ' 
if  choicel  ==  1 

brgbrg; 
elseif  choicel  ==  2 

brgrng; 
elseif  chcicel  ==  3 

rngrng; 
elseif  choicel  =  4 

doppler; 
elseif  choicel  =  5 
shg 
pause 
elseif  choicel  ==  6 
clc 

disp(  '  '); 
disp( 
disp( 
disp( 
disp( 
disp( 
disp( 
disp( 
kmaxs 


0.  Quit'); 
ENTER  YOUR  CHOICE:  '); 


NUMBER  OF  OBSERVATIONS: '); 

%   observation  number 


•) 


ENTER  DESIRED 

'); 

km  ax 

•); 
'); 

PLEASE  MAKE  THIS  GREATER  THAN  5  SO  THAT  THE  KALMAN  GAINS') 

CAN  STABILIZE. ') 

=  input ('kmax  =  ','s'); 
eva 1 ( [ ' kmax  =  ' , kmaxs , ' ; ' ] ) ; 
elseif  choicel  ==  7 
clc 

disp('  '); 

disp( 'ENTER  DESIRED  NUMBER  OF  PREDICTIONS: '); 
disp( '  '  ); 
disp( '  kpred  %   prediction  number  ' ) 


56 


dispC  '); 

dispC  '); 

kpreds  =  input ('kpred  =  ','s'); 

eval([ 'kpred  =  ', kpreds ,';'] ); 
elseif  choicel  ==  8 

clc 

disp( '  '); 

disp( 'ENTER  OBSERVATION  INTERVAL:'); 

dispC  ') 

disp( '  dt 

dispC  '); 

dispC'THE  TIME  INTERVAL  IS  IN  THE  FORM: ') 

dispC  ') 

disp( '  dt  =  time/60' ) 

disp( '  '); 

dispC'FOR  A  10  MINUTE  INTERVAL  ENTER:   dt  =  10/60') 

dispC    •); 

dts  =  inputCdt  =    ','s'); 

eval(['dt  =  ',dts,';']); 
elseif  choicel  =  9 

clc 


%   observation  interval') 


disp( 
disp( 
disp( 
disp( 
disp( 
disp( 
disp( 
disp( 
disp( 
dispC 
disp( 
disp( 
disp( 


') 
') 
') 
•) 
ENTER  THE  FIRST  OBSERVERS  INITIAL  PARAMETERS:  '); 

'); 


'); 


•); 


xfol  =  [ 


X 

vx 

y 

vy  ]; 


%  observer  x  position  (km)  ') 


%   velocity  x  direction  (km/hr) 
%  observer  y  direction  (km)') 
%  velocity  y  direction  (km/hr)') 


') 


YOU  MAY  ENTER  THIS  IN  THE  FOLLOWING  FORMAT:  [  x  vx  y  vy] 
xfols  =  input('xfol  =  ','s'); 
eval(['xfol  =  ', xfols ,';']  ); 
xfol  =  reshape(xfol ,4, 1); 
elseif  choicel  =  10 
clc 

•); 


); 


disp( 
disp( 
disp( 
disp( 
disp( 
disp( 
disp( 
disp( 
disp( 
disp( 
disp( 
disp( 
disp( 


•) 
') 
') 
ENTER  THE  SECONT)  OBSERVERS  INITIAL  PARAMETERS:  '); 

'); 


'); 


'); 


xfo2  =  [ 


X 

vx 

y 
vy  ]; 


%   observer  x  position  (km)') 
%  velocity  x  direction  (km/hr)') 
%  observer  y  direction  (km)') 
%  velocity  y  direction  (km/hr)') 


YOU  MAY  ENTER  THIS  IN  THE  FOLLOWING  FORMAT:  [  x  vx  y  vy] ' '); 
xfo2s  =  input ('xfo2  =  ','s'); 
eval(['xfo2  =  ' ,xfo2s , ' ; ' ] ); 
xfo2  =  reshape(xfo2,4, 1); 
elseif  choicel  =  11 
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clc 
disp( 

disp( 
disp( 
disp( 
disp( 
disp( 


ENTER  OBSERVER  MOVEMENT  SPEED 

'); 

spd 

'); 


%   observer  speed  in  km/hr  ' ) 


•); 


spds  =  inputC'spd  = 
eval( [ ' spd  =  ' ,spds . 
elseif  choicel  =  12 
clc 

'); 


5  s  ); 


disp( 
disp( 
disp( 
disp( 

disp( 
disp( 
disp( 
disp( 
disp( 
disp( 
disp( 
disp( 
disp( 
disp( 
disp( 
disp( 
f  X  vx 


'); 


FOR  THE  PURPOSES  OF  SIMULATION,'); 

ENTER  THE  TARGETS  MOVEMENT  PARAMETERS:  '); 


vx 
ax 

y 

vy 
av 
fo  ]; 

'); 
'); 

YOU  MAY  ENTER  THIS 
ax  y  vy  ay  fo]  '  '  ) 


%  target  initial  x  position  (km)') 
7o   target  initial  x  velocity  (km/hr)') 
%  target  acceleration  in  x  (km/hr^2)') 
%   target  initial  y  position  (km)') 
?o  target  initial  y  velocity  (km/hr)') 
?o  target  acceleration  in  y  (km/hrA2)') 
°o  transmitter  rest  frequency  (Hz)') 


IN  THE  FOLLOWING  FORMAT: 


dispC '  '); 
disp( '  ' ); 
disp(  '  -'.-v.--,vv.--;wVV".'.";.-y.- 

dispC '  "  CAUTION: 
disp(  '  ■'"■'"•'-•',--;.";,--;,-v.",v-,v 

tgts  =  input('tgt  =  ','s'); 
eval([  'tgt  =  '  ,tgts, ';']); 
tgt  =  reshapeC tgt ,7 , 1); 

if  (tgt(2,l)  ==  0  6c  tgt(5,l)  =  0) 


..'.  ^'.  ^'^  .y  w*.  y.  ^  y .  j^  .<^  y^  .y  .'^  ^'..  y.  ..f.  ^'.  .*.  .*.  »*•  j^  ju  y^  y«  <j^  y*  j^  mS*  ij^  h*.  mS^  ^'«  y.  j«  ^<  jL  .J.  jL.  ^^  .J*  ^«  jL 

VELOCITY  OF  THE  TARGET  MUST  NOT  BE  ZERO.  * 


') 
•) 
•) 


clc 

disp( 

disp( 

disp( 

disp( 

disp( 

dispC 

disp( 

disp( 

disp( 

disp( 

disp( 

disp( 

disp( 

disp( 

disp( 

disp( 


'); 
'); 


•); 
•); 


FOR  THE  PURPOSES  OF  SIMULATION,'); 

ENTER  THE  TARGETS  MOVEMENT  PARAMETERS:  '); 


X 

vx 

ax 

y 

vy 

ay 

fo  ]; 


target  initial  x  position  (km)') 
target  initial  x  velocity  (km/hr)') 
target  acceleration  in  x  (km/hrA2)') 
target  initial  y  position  (km)') 
target  initial  y  velocity  (km/hr)') 
target  acceleration  in  y  (km/hrA2)') 
transmitter  rest  frequency  (Hz)') 


'); 

•); 

YOU  MAY  ENTER  THIS  IN  THE  FOLLOWING  FORMAT: 


[x  vx  ax  y  vy  ay  fo]   ); 
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end 
end; 
end; 


dispC  '); 
dispC  •); 

disp(  '  *-V'V*********************TWr**********Vr****************'  ) 

disp('  '■'  CAUTION:  VELOCITY  OF  THE  TARGET  MUST  NOT  BE  ZERO.  *') 

tgts  =  inputCtgt  =  '  ,'s'); 
eval(['tgt  =  ', tgts, •;']); 
tgt  =  reshape(tgt,7,l); 


%   JOHN  A.  HUCKS  II 

%   FILE  NAME  :  BRGBRG. M 

%  EXTENDED  KALMAN  FILTER  PROGRAM  USING  BEARING/BEARING  MEASUREMENTS 

%   THIS  PROGRAM  SIMULATES  THE  TRACK  GENERATED  BY  AN  EXTENDED  KALMAN 
%  FILTER,  WITH  INPUTS  FROM  A  FUSED  SENSOR  ARRAY  AS  IT  TRACKS  A  TARGET 
?o  ACROSS  AN  X-Y  GRID  SPACE  (PLRS  FLAT  EARTH  PROJECTION). 

clg 

clear  eex  eey  xto 
!  erase  brgbrg. met 
diary  bearing 

?o  STATE  EQUATIONS 

%     OBSERVATION  TIME  INTERVAL  (input  from  marine. m) 

%      STATE  TRANSITION  MATRIX 
phi  =  [  1  dt  0   0 

0   10   0 

0   0  1  dt 

0   0  0   1  ]  ; 

%     SYSTEM  NOISE  COEFFICIENT  MATRIX 

del  =  [  df---dt/2  0 

dt  0 

0  dt^'-dt/2 

0  dt    ]  ; 

%   STATE  VARIABLES,  FILTER  ESTIMATES,  TIME,  AND  OBSERVED  DATA 
%     NUMBER  OF  OBSERVATIONS  (input  from  marine. m) 


%     ESTIMATED  TARGET  STATE 

xte  =  zeros(4,kmax);  %  initial  est.  target  state 

%     OBSERVER  (SENSOR)  INITIAL  POSITIONS  AND  VELOCITIES 
%   (input  from  marine. m) 

%  OBSERVER  SPEED  (input  from  marine,  m) 
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%  INITIAL  TIME  SETTING 
Time  =  zeros( 1 ,kmax); 


%  ACTUAL  TRACK  POSITIONS 

X  = 

zeros( 1 jkmax); 

y  = 

zeros( Ijkmax); 

T  = 

0; 

for 

k  = 

1 

x(k)  =  tgtd, 

1); 

y(k)  =  tgt(4, 

1); 

end 

for 

k  = 

2:  kraax 

(tgt  vector  input  from  marine. m) 


end 


T  =  T+dt; 

x(k)  =  (0.5'Vtgt(3,l)*(TA2))+(tgt(2,l).*T)+(tgt(l,l)); 

y(k)  =  (0.5*tgt(6,l)*(TA2))+(tgt(5,l).*T)+(tgt(4,l)); 


zpos 


[ 


%     INPUT  OBSERVATION  VALUES  (BEARING/BEARING) 
%   INITIAL  OBSERVED  TARGET  STATE 
xto(:  ,1)  =  [  tgt(l,l) 

tgt(2,l) 

tgt(4,l) 

tgt(5,l)  ]; 

%  SIMULATION  COUNTER  LOOP 
xfoa  =  zeros(4,kmax); 
xfoa(: ,1)  =  xfol(: ,1); 
xfol  =  xfoa; 
xfob  =  zeros(4,kmax); 
xfob(: ,1)  =  xfo2(: ,1); 
xfo2  =  xfob; 
k  =  0; 

for  kl=l:  lO^kmax 
k  =  k  +  1; 

if  (k  ==  kmax) 
break 

end 


%   (km  in  x  direction) 
%   (km/hr  in  x  direction) 
%  (km  in  y  direction) 
%  (kra/hr  in  y  direction) 


X   actual  loop  counter 
%  check  for  termination 


UPDATE  FORWARD  OBSERVER'S/  FAC'S  POSITION  (IF  THEY  ARE  MOVING) 
if  k  <=  3 

way  =  pi/2; 
else 

way  =  -(3'^pi/4); 
end 
if  spd  =  0 

xfol(l,k+l)  =  xfol(l,k); 

xfol(3,k+l)  =  xfol(3,k): 

xfo2(l,k+l)  =  xfo2(l,k); 

xfo2(3,k+l)  =  xfo2(3,k): 
else 

xfol(l,k+l)  =  xfol(l,k)  +  spd*sin(way)*dt; 

xfol(2,k+l)  =  spd"sin(way); 

xfol(3,k+l)  =  xfol(3,k)  +  spd*cos(way)*dt; 

xfol(4,k+l)  =  spd"''cos(way); 
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xfo2(l,k+l)  =  xfo2(l,k)  +  spd*sin(way)*dt; 

xfo2(2,k+l)  =  spd'^sin(way); 

xfo2(3,k+l)  =  xfo2(3,k)  +  spd*cos(way)*dt; 

xfo2(4,k+l)  =  spd*cos(way); 
end 
end 

deltlx  =  2pos(l,:)  -  xfol(l,:); 
deltly  =  zpos(2,:)  -  xfol(3,:); 
delt2x  =  zposCl,:)  -  xfo2(l,:); 
delt2y  =  zpos(2,:)  -  xfo2(3,:); 
brgl  =  atan2(deltlx, deltly); 
brg2  =  atan2(delt2x,delt2y); 
zobs  =  [brgl;brg2]  ; 
xtol  =  zeros(4,kinax); 
xtol(l,:)  =  zpos(l,:); 
xtol(3,:)  =  zpos(2,:); 
xtol(: , 1)  =  xto(: , 1); 
xto  =  xtol; 


%   bearing  thtal 
%   bearing  thta2 


%     STATE  EXCITATION  AND  MEASUREMENT  ERROR  COVARIANCE  MATRICES 

varv  =  0. 0001;  %  variance  of  linear  acceleration 


varth  =  0. 01096; 
R  =  [  0. 000005   0 

0   0.  000005  ]  ; 


%   variance  of  angular  acceleration 
%   measurement  noise 


%      INPUT  OBSERVATION  VALUES  WITH  NOISE  ADDED 

rand( ' normal ' ) 

zobs  =  zobs  +  sqrt(R(  1 , 1)  )'''rand(zobs); 

%  INITIAL  KALMAN  MATRICES  AND  OBSERVATION 
I  =  eye(4); 


%     INITIAL  KALMAN  P  MATRIX 
PO  =  [  800   0   0    0 
0    800  0    0 
0    0   800   0 
0    0   0   800  ]; 

%     REINITIALIZING  P  MATRIX 
P  =  PO; 

%     KALMAN  ESTIMATE 
xte(: ,1)  =  [  xto(l,l) 
tgt(2,l) 
xto(3,l) 
tgt(5,l)  ]; 

tpred  =  xte( : , 1); 


reset  value  of  Error  Covariance 


%  (km  -  X  direction) 
%  (km/hr) 

%  (km  -  y  direction) 
%  (kra/hr) 


%     SIMULATION  OF  KALMAN  FILTER  OPERATION 

%     NUMBER  OF  BAD  OBSERVATIONS 

nbad  =0;  %  initial  setting  bad  obs.  cntr. 

%     SIMULATION  COUNTER  LOOP 

k  =  0; 

for  kl=l:  10"kmax 
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k  =  k  +  1; 

if  (k  ==  kmax) 

break 
end 


%  actual  loop  cotinter 
%  check  for  termination 


UPDATE  FORWARD  OBSERVER'S/  FAC'S  POSITION  (IF  THEY  ARE  MOVING) 
if  k  <=  3 

way  =  pi/2; 
else 

way  =  -(3'^pi/4); 
end 
if  spd  =  0 

xfol(l,k+l)  =  xfol(l,k); 

xfol(3,k+l)  =  xfol(3,k); 

xfo2(l,k+l)  =  xfo2(l,k); 

xfo2(3,k+l)  =  xfo2(3,k); 
else 

xfol(l,k+l)  =  xfol(l,k)  +  spd*sin(way)*dt; 

xfol(2,k+l)  =  spd-'''sin(way); 

xfol(3,k+l)  =  xfol(3,k)  +  spd'^cos(way)*dt; 

xfol(4,k+l)  =  spd"Cos(way); 

xfo2(l,k+l)  =  xfo2(l,k)  +  spd*sin(way)'Vdt; 

xfo2(2,k+l)  =  spd'''sin(way); 

xfo2(3,k+l)  =  xfo2(3,k)  +  spd*cos(way)'Mti 

xfo2(4,k+l)  =  spd"''cos(way); 
end 


%  PROJECT  AHEAD  KALMAN  STATE  ESTIMATE 
xte(:  ,k+l)  =  phi"'^xte(:  ,k); 


%     UPDATE  Q 

vt  =  sqrt((xte(2,k+l)) 


qll  = 
q22  = 
ql2  = 
q21  = 
Ql  = 


varv 
varv 


(xte(2,k+l) 
(xte(4,k+l) 


xte(2,k+l)  '• 
ql2; 
qll  ql2 
q21  q22  ]; 
Q  =  del-'^Ql->(der  ); 


+  (xte(4,k+l))A2); 

/  vt)A2  +  varth  *   (xte(4,k+l) ) a2; 

/  vt)A2  +  varth  *   (xte(2,k+l) ) a2; 


xte(4,k+l)  *  ((varv/vt)A2  -  varth  ); 


%     PROJECT  ERROR  COVARIANCE 
P  =  phi--'^P-'>(phi')  +  Q; 


%  UPDATE  H 
deltlx  =  xte(l,k+l) 
deltly  =  xte(3,k+l) 
delt2x  =  xte(l,k+l) 
delt2y  =  xte(3,k+l) 


rl2  =  (deltlx) 
r22  =  (delt2x) 
rl  = 
r2  = 

H  =  I 


sqrt(rl2); 

sqrt(r22); 
deltly/rl2 
delt2y/r22 


-  xfol(l,k+l); 

-  xfol(3,k+l); 

-  xfo2(l,k+l); 

-  xfo2(3,k+l); 
(deltly)  2; 
(delt2y)  2; 


■deltlx/rl2 
•delt2x/r22 


]; 


%  GET  NEW  MEASURED  ESTIMATE 
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zhatl  =  atan2(deltlx,deltly);  %   bearing  estimate  thtal 

2hat2  =  atan2(delt2x,delt2y);  %   bearing  estimate  thta2 

zhat  =  [  zhatl 

zhat2  ] ; 

%  COMPUTATION  OF  KALMAN  GAIN 

vresid  =  (H"P-''<-(H' )  +  R);  %   variance  of  the  residual 

G  =  P'HH')  /  vresid;  %  Kalman  gain 

%     COMPUTATION  OF  UPDATED  ERROR  COVARIANCE 
P  =  (I  -  G'^H)*P; 

%  COMPUTATION  OF  RESIDUAL 
resid  =  zobs(: ,k+l)  -  zhat; 

%  IF  RESIDUAL  IS  TOO  BIG,  THEN  RESET  P  AND  DO  CALCULATIONS  AGAIN, 
if  (abs(resid(l))  >  1. 0*sqrt( abs(vresid( 1 , 1) ) )  ... 

]  abs(resid(2))  >  1. 0*sqrt( abs( vresid(2,2)))  ... 

&  nbad  =0) 

nbad  =  1; 

k  =  k-1; 

P  =  PO;  /o  reset  P 

else 
%     COMPUTATION  OF  UPDATED  ESTIMATE 

xte(:  ,k+l)  =  xte(:  ,k+l)  +  G''-resid; 

?o  ERROR  ELLIPSOID  PLOTTING  ROUTINE 
M  =  [  xte(l,k+l)  xte(3,k+l)  ]'; 
K  =  [  P(l,l)  P(l,3) 

P(3,l)  P(3,3)  ]; 
[xp.yp]  =  errellip(M,K,. 60,50,0); 
eex( :  ,k)  =  xp; 
eey(:  ,k)  =  yp; 

%     PREDICTED  (FUTLTIE  TRACK)  USING  KALMAN  ESTIMATE 
%  (number  of  predictions  desired  input  from  marine. m) 
tpred(: ,1)  =  xte(: ,k+l); 
ace  =  zeros(2,l); 

for  1  =  Irkpred  %  kpred  is  //  of  predictions 

if  k  >=  3 

accx  =  (xte(2,k+l)-xte(2,k))/dt; 
accy  =  (xte(4,k+l)-xte(4,k))/dt; 
ace  =  [ accx; accy] ; 
if  accx  >=  5 
break 

ace  =  zeros(2, 1); 
elseif  accy  >=  5 
break 

ace  =  zeros( 2,1); 
end 
end 

tpred(: ,1+1)  =  phi*tpred( : ,l)+del*acc; 
end 

nbad  =  0; 
end 
%  PLOT  OBSERVED  AND  ESTIMATED  TRACKS  (TARGET  AND  OBSERVER) 


63 


plot(xfol(l,l:k+l),xfol(3,l:k+l),'xw'  ,xto(  1, 1:  k+1)  ,xto(3, 1:  k+1) 
,  .  b  ,.  .  . 

xte(l,l:k+l),xte(3,l:k+l),'+w'  ,xfo2(  1 , 1:  k+1)  ,xfo2(3, 1:  k+1)  'xw'  ,.  .. 
,  zpos(l,l:k+l),zpos(2,l:k+l),'ow'  ,tpred(l,:  ),tpred(3,:  ),'*g  ,.  .  . 
eex.eey , ' -r ' ) ,grid 
t it le( 'ACTUAL/OBSERVED/ESTIMATED/PREDICTED  TGT  TRKS  USING  BRG/BRG  ') 

xialDeK 'DISTANCE  X' )  ,ylabel( 'DISTANCE  Y') 
end  • 

meta  brgbrg 


%   JOHN  A.  HUCKS  II 

%  FILE  TITLE:  RNGRNG. M 

%   EXTENDED  KALMAN  FILTER  PROGRAM  USING  RANGE/RANGE  MEASUREMENTS 

o/ 
/o 

%  THIS  PROGRAM  SIMULATES  THE  TRACK  GENERATED  BY  AN  EXTENDED  KALMAN 
%  FILTER,  WITH  INPUTS  FROM  A  FUSED  SENSOR  ARRAY  AS  IT  TRACKS  A  TARGET 
?o  ACROSS  AN  X-Y  GRID  SPACE  (PLRS  FLAT  EARTH  PROJECTION). 

clg 

clear  eex  eey  xto 
! erase  rngrng. met 
diary  rngrng 

%  STATE  EQUATIONS 

%     OBSERVATION  TIME  INTERVAL  (input  from  marine. m) 

%  STATE  TRANSITION  MATRIX 
phi  =  [  1  dt  0   0 

0   10   0 

0   0  1  dt 

0   0  0   1]; 

%  SYSTEM  NOISE  COEFFICIENT  MATRIX 

del  =  [  dt'--dt/2  0 

dt  0 

0  dt-dt/2 

0  dt    ]; 

%     STATE  VARIABLES,  FILTER  ESTIMATES,  TIME,  AND  OBSERVED  DATA 

%     NUMBER  OF  OBSERVATIONS  (input  from  marine,  m) 

%  ESTIMATED  TARGET  STATE 

xte  =  zeros(4,kmax);  %   initial  est.  target  state 

%     OBSERVER  (SENSOR)  INITIAL  POSITIONS  AND  VELOCITIES 
%  (input  from  marine. m) 


%  OBSERVER  SPEED  (input  from  marine. m) 
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%      INITIAL  TIME  SETTING 
Time  =  zeros( 1 ,kmax); 

%  ACTUAL  TRACK  POSITIONS  (tgt  vector  input  from  marine. m) 
X  =  zeros ( Ijkmax); 
y  =  zerosC Ijkmax); 
T  =  0; 
for  k  =  1 

x(k)  =  tgt(l,l); 

y(k)  =  tgt(4,l); 


end 
for  k 


end 
zpos  =  [ 


2:  kmax 

T  =  T+dt; 

x(k)  =  (0.5*tgt(3,l)*(TA2))+(tgt(2,l).*T)+(tgt(l,l)); 

y(k)  =  (0.5*tgt(6,l)*(TA2))+(tgt(5.1).*T)+(tgt(4,l)); 

X 

y  ]; 


%     INPUT  OBSERVATION  VALUES  (RANGE/RANGE) 
°o  OBSERVED  TARGET  STATE 
xfoa  =  zeros(4 jkraax); 
xfoa(: ,1)  =  xfol(: ,1); 
xfol  =  xfoa; 
xfob  =  zeros(4,kmax); 
xfob(: ,1)  =  xfo2(: ,1); 
xfo2  =  xfob; 
xto(: ,1)  =  [  tgt(l,l) 
tgt(2,l) 
tgt(4,l) 
tgt(5,l)  ]; 
%     SIMULATION  COUNTER  LOOP 
k  =  0; 

for  kl=l: 10"kmax 
k  =  k  +  1; 
if  (k  =  kmax) 

break 
end 


%  (km  in  x  direction) 
%  (kra/hr  in  x  direction) 
%  (km  in  y  direction) 
%  (km/hr  in  y  direction) 


%  actual  loop  counter 
%   check  for  termination 


UPDATE  FORWARD  OBSERVER'S/  FAC'S  POSITION  (IF  THEY  ARE  MOVING) 
if  k  <=  3 

way  =  pi/2; 
else 

way  =  -(3^-pi/4); 
end 
if  spd  =  0 

xfol(l,k+l 

xfol(3,k+l 

xfo2(l,k+l 

xfo2(3,k+l 
else 

xfol(l,k+l 

xfol(2,k+l 

xfol(3,k+l 

xfol(4,k+l 

xfo2(l,k+l 


=  xfol(l,k) 
=  xfol(3,k) 
=  xfo2(l,k) 
=  xfo2(3,k) 


=  xfol(l,k)  +  spd*sin(way)*10*dt 

=  spd"sin(way); 

=  xfol(3,k)  +  spd*cos(way)*10''-dt 

=  spd" cos (way); 

=  xfo2(l,k)  +  spd*sin(way)*10*dt 
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xfo2(2,k+l)  =  spd*sin(way); 

xfo2(3,k+l)  =  xfo2(3,k)  +  spd*cos(way)*10*dt; 

xfo2(4,k+l)  =  spd'"fcos(way); 
end 
end 

rl  =  sqrt((zpos(l,:  )-xfol(l,:  )).  a2  +  (zpos(2,:  )-xfol(3,:  )).  a2); 
r2  =  sqrt((zpos(l,:  )-xfo2(l,:  )).  a2  +  (zpos(2,:  )-xfo2(3,:  )).  a2); 
zobs=[  rl;  r2]  ; 
xtol  =  zeros(4,kmax); 
xtol(l,:)  =  zpos(l,:); 
xtol(3,:  )  =  zpos(2,:  ); 
xtol(: ,1)  =  xto(: ,1); 
xto  =  xtol; 

%  STATE  EXCITATION  AND  MEASUREMENT  ERROR  COVARIANCE  MATRICES 

varv  =5.0;  %  variance  of  linear  acceleration 

varth  =1.0;  %   variance  of  angular  acceleration 

R  =  [  0.  005   0 

0  0. 005  ] ;  %  measurement  noise 

%  INPIT  OBSERVATION  VALUES  WITH  NOISE  ADDED 

rand( 'normal ' ); 

zobs  =  zobs  +  sqrt(R( 1 , 1) )"rand(zobs); 

%      INITIAL  KALMAN  MATRICES  AND  OBSERVATION 
I  =  eye(4); 

%     initial' KALMAN  P  MATRIX 

PO  =  [  250  0  0   0  %  reset  value  of  Error  Covariance 

0    250  0    0 
0    0   250   0 
0   0   0   250  ] ; 
%  REINITIALIZING  P  MATRIX 
P  =  PO; 

%  KALMAN  ESTIMATE 

xte(:  ,1)  =  [  xto(l,l)  %  (km  -  x  direction) 

tgt(2,l)  %  (km/hr) 

xto(3,l)  %  (km  -  y  direction) 

tgt(5,l)  ];  %  (km/hr) 

tpred  =  xte(: ,1); 

%  SIMULATION  OF  KALMAN  FILTER  OPERATION 

%  NUMBER  OF  BAD  OBSERVATIONS 

nbad  =0;  %  initial  setting  bad  obs.  cntr. 

%     SIMULATION  COUNTER  LOOP 

k  =  0; 

for  kl=l:  lO'^kraax 

k  =  k  +  1;  %   actual  loop  counter 

if  (k  =  kmax)  %  check  for  termination 

break 
end 

%  UPDATE  FORWARD  OBSERVER'S/  FAC'S  POSITION  (IF  THEY  ARE  MOVING) 
if  k  <=  3 
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way  = 

else 

way  = 

end 

if  spd  = 
xfoK 
xfol( 
xfo2( 
xfo2( 

else 
xfoK 
xfoK 
xfol( 
xfol( 
xfo2( 
xfo2( 
xfo2( 
xfo2( 

end 


pi/2; 

-(3'Vpi/4); 

=  0 

l,k+l)  =  xfol(l,k); 
3,k+l)  =  xfol(3,k); 
l,k+l)  =  xfo2(l,k); 
3,k+l)  =  xfo2(3,k); 

l,k+l)  =  xfol(l,k)  +  spd*sin(way)*10*dt 

2,k+l)  =  spd*sin(way); 

3,k+l)  =  xfol(3,k)  +  spd*cos(way)*10*dt 

4,k+l)  =  spd*cos(way); 

l,k+l)  =  xfo2(l,k)  +  spd*sin(way)*10*dt 

2,k+l)  =  spd"sin(way); 

3,k+l)  =  xfo2(3,k)  +  spd*cos(way)*10*dt 

4,k+l)  =  spd" cos (way); 


%     PROJECT  AHEAD  KALMAN  STATE  ESTIMATE 

xte(:  ,k+l)  =  phi''^xte(:  ,k); 

%  UPDATE  Q 

vt  =  sqrt((xte(2,k+l))  2  +  (xte(4,k+l))  2); 

qll  =  varv  '•  (xte(2,k+l)  /  vt)  2  +  varth  *  (xte(4,k+l))  2; 

q22  =  varv  *   (xte(4,k+l)  /  vt)  2  +  varth  *  (xte(2,k+l))  2; 

ql2  =  xte(2,k+l)  '•  xte(4,k+l)  *   ((varv/vt)  2  -  varth  ); 

q21  =  ql2; 

Ql  =  [  qll  ql2 


Q  =  de 


a21  q22  1; 
l-->Ql'V(der); 


%  PROJECT  ERROR  COVARIANCE 
P  =  phi-"-p-->(phi')  +  Q; 

%     UPDATE  H 

deltlx  =  xte(l,k+l)  -  xfol(l,k+l) 
deltly  =  xte(3,k+l)  -  xfol(3,k+l) 
delt2x  =  xte(l,k+l)  -  xfo2(l,k+l) 
delt2y  =  xte(3,k+l)  -  xfo2(3,k+l) 
rl2  =  (deltlx)  2  +  (deltlv)  2; 
r22  =  (delt2x)  2  +  (delt2y)  2; 
rl  =  sqrt(rl2); 
r2  =  sqrt(r22); 

H  =  [   deltlx/rl   0   deltly/rl 
delt2x/r2   0   delt2y/r2 

%     GET  NEW  MEASURED  ESTIMATE 
zhat  =  [  rl 

r2  ]; 

%     COMPUTATION  OF  KALMAN  GAIN 
vresid  =  (H--''P'>(H' )  +  R); 
G  =  P^'--(H')  /  vresid; 


0 
0  ]; 


%  variance  of  the  residual 
%   Kalman  gain 
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%     COMPUTATION  OF  UPDATED  ERROR  COVARIANCE 
P  =  (I  -  G^H)''^P; 

%  COMPUTATION  OF  RESIDUAL 
resid  =  zobs(: ,k+l)  -  zhat; 

X     IF  RESIDUAL  IS  TOO  BIG,  THEN  RESET  P  AND  DO  CALCULATIONS  AGAIN, 
if  (abs(resid(l))  >  1.  0''^sqrt(  abs(vresid(  1, 1) ))  ... 

]  abs(resid(2))  >  1. 0*sqrt( abs(vresid(2,2)) )  ... 
&  nbad  =  0) 
nbad  =  1; 
k  =  k-1; 

P  =  PO;  %  reset  P 

else 

%     COMPUTATION  OF  UPDATED  ESTIMATE 
xte(: ,k+l)  =  xte(: ,k+l)  +  G-resid; 

%     ERROR  ELLIPSOID  PLOTTING  ROUTINE 
M  =  [xte(l,k+l)  xte(3,k+l)]  '; 
K  =  [  P(l,l)  P(l,3) 

P( 3  ,  1)  P( 3 ,3)  ] ; 
[xp,yp]  ='errellip(M,k,.  60,25,0); 
eex( :  ,k)  =  xp; 
eey(:  ,k)  =  yp; 

%  PREDICTED  (FUTURE  TRACK)  USING  KALMAN  ESTIMATE 
%     (number  of  predictions  desired  input  from  marine,  m) 
tpred(:  ,1)  =  xte(:  ,k+l); 
ace  =  zeros( 2,1); 

for  1  =  1:  kpred  %  kpred  is  i^   of  predictions 

if  k  >=  5 

accx  =  (xte(2,k+l)-xte(2,k))/dt; 
accy  =  (xte(4,k+l)-xte(4,k))/dt; 
ace  =  [ accx; accy] ; 
if  accx  >=  5 
break 

ace  =  [  0;  0]  ; 
elseif  accy  >=  5 
break 

ace  =  [  0;  0]  ; 
end 
end 

tpred(: ,1+1)  =  phi*tpred(: ,l)+del*acc; 
end 

nbad  =  0; 
end 

%  PLOT  OBSERVED  AND  ESTIMATED  TRACKS  (TARGET  AND  OBSERVER) 
plot(xfol(l,l:k+l),xfol(3,l:k+l),'xw'  ,xte(l ,  1:  k+1)  , 
xte(3,l:k+l),'+w'  ,.  .  . 

xfo2(l,l:k+l),xfo2(3,l:k+l),  'xw'  ,zpos(  1 , 1:  k+1)  ,zpos(2 , 1:  k+1)  , '  ow'  ,.  .  . 
xto(l,l:k+l),xto(3,l:k+l),'.b'  ,tpred(l,:  ),tpred(3,:  ),'*g'  ,.  .  . 
eex,eey , ' -r' ) ,grid 
t it le( 'ACTUAL/OBSERVED/ESTIMATED/PREDICTED  TGT  TRKS 
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USING  RNG/RNG  OBS'),. .  . 

xlabeK 'DISTANCE  X' )  ,ylabel( 'DISTANCE  Y' ) 
end 

meta  rngrng 
diary  off 


%     JOHN  A.  HUCKS  II 

%     FILE  NAME  :  BRGRNG.  M 

X     EXTENDED  KALMAN  FILTER  USING  BEARING/RANGE  MEASUREMENTS 

%  THIS  PROGRAM  SIMULATES  THE  TRACK  GENERATED  BY  AN  EXTENDED  KALMAN 

%  FILTER  WITH  INPUTS  FROM  A  FUSED  SENSOR  ARRAY,  AS  IT  TRACKS  A 

%  POTENTIAL  TARGET  ACROSS  AN  X-Y  GRID  SPACE  (PLRS  FLAT  EARTH 

%  PROJECTION). 

clg 

clear  eex  eey  xto 
! erase  brgrng. met 
diary  brgrng 

%  STATE  EQUATIONS 

%  OBSERVATION  TIME  INTERVAL  (input  from  marine. m) 

%     STATE  TRANSISTION  MATRIX 
phi  =  [  1  dt  0   0 

0   10   0 

0   0  1  dt 

0   0  0   1  ]  ; 

%  SYSTEM  NOISE  COEFFICIENT  MATRIX 
del  =  [  dt----dt/2    0 

dt       0 

0      dt^Mt/2 

0        dt    ]  ; 

%  STATE  VARIABLES,  FILTER  ESTIMATES,  TIME,  AND  OBSERVED  DATA 
%  NUMBER  OF  OBSERVATIONS  (input  from  marine. m) 

%     ESTIMATED  TARGET  STATE 

xte  =  zeros(4,kmax);  %  estimated  target  state 

%  OBSERVER  (SENSOR)  INITIAL  POSITIIONS  AND  VELOCITIES  (input  from  marine. m) 

%  OBSERVER  SPEED  (input  from  marine. m) 

%  INITIAL  TIME  SETUP 
Time  =  zeros( l,kmax); 

%  ACTUAL  TRACK  POSITIONS  (tgt  vector  input  from  marine. m) 
X  =  zeros( 1 jkmax); 
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y  = 

zeros ( Ijkmax); 

T  = 

0; 

for 

k  = 

1 

■ 

x(k)  = 

tgtd, 

1); 

y(k)  = 

tgt(4 

1); 

end 

for 

k  = 

2:  kraax 

end 


T  =  T+dt; 

x(k)  =  (0.5'Vtgt(3,l)'>(TA2))+(tgt(2,l)*T)+(tgt(l,l5); 

y(k)  =  (0.5'Vtgt(6,l)'V(TA2))+(tgt(5,l)*T)+(tgt(4,l)); 


zpos  =  [ 


X 

y  ]; 


%   INPUT  OBSERVATION  VALUES  (BEARING/RANGE): 

xfoa  =  2eros(4,kmax); 

xfoa(: ,1)  =  xfol(: ,1); 

xfol  =  xfoa; 

xfob  =  zeros(4,kmax); 

xfob(: ,1)  =  xfo2(: ,1); 

xfo2  =  xfob; 

xfor  =  zeros( 4 ,kmax); 

zobr  =  zerosC 1: kmax) ; 

rngl  =  zeros( 1: kmax); 

rng2  =  zercs( 1: kmax); 

deltlx  =  zeros( 1: kmax); 

delt2x  =  zeros( 1: kmax); 

deltly  =  zeros( 1:  kmax); 

delt2y  =  zeros( 1:  kmax); 

thtal  =  zeros( 1: kmax); 

thta2  =  zeros( 1: kmax); 

for  k  =  1: kmax 

rngl(k)  =  sqrt(((zpos(l,k)-xfol(l,k)).  a2)  + 
((zpos(2,k)-xfol(3,k)).  a2)); 

rng2(k)  =  sqrt( ( (zpos( 1 ,k) -xfo2(l ,k) ).  a2)  + 
((zpos(2,k)-xfo2(3,k)).  a2)); 

deltlx(k)  =  zpos( l,k)-xfol( l,k); 

deltly(k)  =  zpos( 2 ,k) -xf ol( 3 ,k)  ; 

delt2x(k)  =  zpos(l,k)-xfo2(l,k); 

delt2y(k)  =  zpos( 2 ,k) -xfo2( 3 ,k); 

thtal(k)  =  atan2(deltlx(k) ,deltly(k)); 

thta2(k)  =  atan2(delt2x(k),delt2y(k)); 
end 
zobr  =  [  thtal; rngl; thta2; rng2  ]  ; 


%   MEASUREMENT  REFERENCE  CONVERSION  ROUTINE: 
for 


cnt  =  1 

xtl  = 

tgt(l,l); 

xt2  = 

tgt(l,l); 

ytl  = 

tgt(4,l); 

yt2  = 

tgt(4,l); 

xtr  = 

(xtl+xt2)/2; 

ytr  = 

(ytl+yt2)/2; 

delxr 

=  xtr( 1 ,cnt); 

delyr 

=  ytr(l,cnt); 

thtar(l,cnt)  =  atan2(delxr, delyr); 
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rngr(] 

.,cnt)   = 

=  sqrt( 

end 

for 

cnt  =  2: 

kmax 

xtl(l, 

cnt)   = 

zobr(2 

ytid, 

cnt)   = 

2obr(2 

xt2(l, 

cnt)   = 

zobr(4 

yt2(i, 

cnt)   = 

zobr(4 

xtr(l, 

cnt)   = 

(xtld 

ytrd, 

cnt)   = 

(yti(i 

delxr 

=  xtr(] 

L,cnt); 

delyr 

=  ytr(] 

[,cnt); 

thtar(l,cnt) 

=  atan 

rngr( ] 

,cnt)   = 

=  sqrt( 

end 


=  sqrt((delxr  2)+(delyrA2) ); 


,cnt)*sin(zobr( l,cnt)) 

,cnt)^"cos(zobr(  l,cnt)) 

,cnt)*sin(zobr(3,cnt)) 

,cnt)*cos(zobr(3,cnt)) 

,cnt)+xt2(l,cnt))/2; 

,cnt)+yt2(l,cnt))/2; 


2(delxr, delyr); 
(delxr  2)+(delyrA2)); 


%  (km  in  x  direction) 
%   (km/hr  in  x  direction) 
%  (km  in  y  direction) 
%  (km/hr  in  y  direction) 


%   CONVERTED  OBSERVATION  MATRIX:  (CONVERSION  OBSERVER  TAKEN  AS  THE  ORIGIN) 
%     OBSERVED  TARGET  STATE 
xto(:  ,1)  =  [  tgt(l,l) 

tgt(2,l) 

tgt(4,l) 

tgt(5,l)  ]; 
zobs  =  [  thtar 

rngr  ] ; 
xtol  =  zeros(4,kmax); 
xtol(l,:)  =  2pos(l,:); 
xtol(3,:)  =  zpos(2,:); 
xtol(: , 1)  =  xto(: , 1); 
xto  =  xtol; 


%     STATE  EXCITATION  AND  MEASUREMENT  ERROR  COVARIANCE  MATRICES 
varv  =  5.  0;  %  variance  of  linear  acceleration 

varth  =1.0;  %   variance  of  angular  acceleration 

R  =  [   0.  0005   0 

0       0. 005  ] ;  %  measurement  noise 

?o  INPUT  OBSERVATION  VALUES  WITH  NOISE  ADDED 

rand( ' normal '  ) 

noise  =  sqrt(R(  1 , 1)  )'^i^and(zobs); 

%  INITIAL  KALMAN  MATRICES  AND  OBSERVATIONS 

I  =  eye(4); 

%     INITIAL  KALMAN  P  MATRIX 

PO  =  [  700   0   0   0 

0   700   0   0 

0   0   700   0 

0   0   0   700  ]; 


%  reset  value  of  error  covariance 


%  REINITIALIZING  P  MATRIX 
P  =  PO; 

%  INITIAL  KALMAN  ESTIMATE 
xte(:  ,1)  =  [  xto(l,l) 
tgt(2,l) 
xto(3,l) 
tgt(5,l)  ]; 
tpred  =  xte(:  ,1); 
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%     SIMULATION  OF  FUSED  SENSOR  OPERATION  USING  EXTENDED  KALMAN  FILTER 

%  NUMBER  OF  BAD  OBSERVATIONS 

nbad  =  0;  %  initial  setting  bad  obs.  cntr. 

k  =  0; 

for  kl=l:  50*kmax 

k  =  k  +  1;  %   actual  loop  counter 

if  (k  =  kmax)  X   check  for  termination 

break 
end 


%  UPDATE  OBSERVER'S /FAG'S  POSITION  (IF  THEY  ARE  MOVING) 
if  k  <=  3 

way  =  pi/2; 
else 

way  =  -pi/4; 
end 
if  spd  =  0 

xfol(l,k+l) 

xfol(3,k+l) 

xfo2(l,k+l) 

xfo2(3,k+l) 
else 

xfol(l,k+l) 

xfol(2,k+l) 

xfol(3,k+l) 

xfol(4,k+l) 

xfo2(l,k+l) 

xfo2(2,k+l) 

xfo2(3,k+l) 

xfo2(4,k+l) 
end 


xfol(l,k) 
xfol(3,k) 
xfo2(l,k) 
xfo2(3,k) 


xfol(l,k)  +  spd*sin(way)*dt 

spd"sin(way); 

xfol(3,k)  +  spd*cos(way)*dt 

spd"Cos(way); 

xfo2(l,k)  +  spd'^sin(way)*dt 

spd"sin(way); 

xfo2(3,k)  +  spd*cos(way)*dt 

spd"Cos(way); 


%  PROJECT  AHEAD  KALMAN  STATE  ESTIMATE 
xte(:  ,k+l)  =  phi*xte(:  ,k); 


%  UPDATE  Q 

vt  =  sqrt((xte(2,k+l))A2  +  (xte(4,k+l) ) a2); 

varv  '^   (xte(2,k+l)  /  vt)A2  +  varth  *  (xte(4,k+l))A2; 

varv  *  (xte(4,k+l)  /  vt)A2  +  varth  *   (xte(2,k+l))A2; 

xte(2,k+l)  -^   xte(4,k+l)  *  ((varv/vt)A2  -  varth  ); 

ql2; 
qll  ql2 


qll 
q22 
ql2  = 
q21  = 
Ql  = 


Q  =  de 


q21  q22  1; 
l'>Ql*(der  ); 


%  PROJECT  ERROR  COVARIANCE 
P  =  phi*P*(phi')  +  Q; 


%  UPDATE  H 
deltx  =  xte(l,k+l); 
deity  =  xte(3,k+l); 
r2  =  (deltx)  2  +  (deity)  2; 
r  =  sqrt(r2); 

H  =  [   delty/r2   0    -deltx/r2 
deltx/r   0    delty/r 
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%  GET  NEW  MEASURED  ESTIMATE 

delx  =  xte( l,k+l); 

dely  =  xte(3,k+l); 

zhatl  =  atan2(delx,dely); 

zhat2  =  sqrt(delxA2  +  delyA2); 

zhat  =  [  zhatl 

zhat2  ]  ; 

%     COMPUTATION  OF  KALMAN  GAIN 

vresid  =  (H'^P"(H')  +  R);  %   variance  of  the  residual 

G  =  P*(H')  /  vresid;  %  Kalman  gain 

%  COMPUTATION  OF  PROJECTED  ERROR  COVARIANCE 
P  =  (I  -  G*H)''^P; 

%  COMPUTATION  OF  RESIDUAL 
resid  =  zobs(: ,k+l)  -  zhat; 

%     IF  RESIDUAL  TOO  BIG,  RESET  P  AND  DO  CALCULATIONS  AGAIN. 
if  (abs(resid(l))  >  1.  0''-sqrt(  abs(  vresid(l ,  1) ) )  ... 

]  abs(resid(2))  >  1.  0"sqrt(abs( vresid( 2,2) ) )  ... 
&  nbad  <  2) 

nbad  =  nbad  +  1; 

k  =  k-1; 

P  =  PO;  %  reset  P 

else 

%  COMPUTATION  OF  UPDATED  ESTIMATE 

xte(:  ,k+l)  =  xte(:  ,k+l)  +  G'^resid; 

%     ERROR  ELLIPSOID  PLOTTING  ROUTINE 
M  =  [xte(l,k+l)  xte(3,k+l)]  '; 
K  =  [  P(l,l)  P(l,3) 

P( 3 , 1)  P(3,3)  ]  ; 
[xp,yp]  ='errellip(M,K,.  60,25,0); 
eex( : ,k)  =  xp; 
eey(: ,k)  =  yp; 

X     PREDICTED  (FUTURE  TRACK)  USING  KALMAN  ESTIMATE 
%     (number  of  predictions  desired  input  from  marine. m) 
tpred(: ,1)  =  xte(: ,k+l); 
ace  =  zeros(2,l); 

for  1  =  1:  kpred  %  kpred  is  y/  of  predictions 

if  k  >=  5 

accx  =  (xte(2,k+l)-xte(2,k))/dt; 
accy  =  (xte(4,k+l)-xte(4,k))/dt; 
ace  =  [ accx; accy] ; 
if  accx  >=  5 
break 

ace  =  zeros(2,l); 
elseif  accy  >=  5 
break 

ace  =  zeros(2, 1); 
end 
end 
tpred(:  ,1+1)  =  phi''-tpred( :  ,  l)+del'^acc; 
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end 

nbad  =  0; 

end 

%  PLOT  OBSERVED  AND  ESTIMATED  TRACKS  (TARGET  AND  OBSERVER) 

plot(xfol(l,l:k+l),xfol(3,l:k+l),'xw'  ,xfo2(  1, 1:  k+1)  , 

xfo2(3,l:k+l),'xw'  ,.  .  . 

xto(l,l:k+l),xto(3,l:k+l),'.b'  ,xte(  1, 1:  k+1)  ,xte(  3, 1:  k+1)  , '+w'  ,, 

zpos(l,l:  k+l),zpos(2,l:k+l),  'ow'  ,tpred(l,:  ),tpred(3,:  ),'*g'  ,.  .  . 

eex  eev   ~r  )  srid 

t it ie( 'ACTUAL/OBSERVED/ESTIMATED/PREDICTED  TGT  IRKS 

USING  BRG/RNG  OBS'  ),.  .  . 

xlabeK 'DISTANCE  X' )  ,ylabel( 'DISTANCE  Y') 
end 

diary  off 
meta  brgrng 


%   JOHN  A.  HUCKS  II 

?c  FILE  NAME  :  DOPPLER.  M 

%   EXTENDED  KALMAN  FILTER  PROGRAM  USING  A  DOPPLER  SHIFTED  FREQUENCY 

%  SIGNAL  AND  THE  BEARING  TO  THAT  SIGNAL. 

%   THIS  PROGRAM  SIMULATES  THE  TRACK  GENERATED  BY  AN  EXTENDED  KALMAN 
%   FILTER,  WITH  INPUTS  FROM  A  FUSED  SENSOR  ARRAY  AS  IT  TRACKS   A  TARGET 
%   ACROSS  AN  X-Y  GRID  SPACE  (PLRS  FLAT  EARTH  PROJECTION). 

clg 

clear  eex  eey  xto 
! erase  doppler.  met 
diary  doppler 

%  STATE  EQUATIONS 

%     OBSERVATION  TIME  INTERVAL  (input  from  marine,  m) 

%     STATE  TRANSITION  MATRIX 
phi  =  [  1   dt   0   0   0 

0    10   0   0 

0   0   1   dt   0 

0   0   0    10 

0    0   0    0    1  ]; 

%  SYSTEM  NOISE  COEFFICIENT  MATRIX 

del  =  [  dt   dt''fdt/2   0     0  0 

0      dt      0     0  0 

0      0      dt   dt'Mt/2  0 

0      0       0     dt  0 

0      0       0     0  dt  ]  ; 

%  STATE  VARIABLES,  FILTER  ESTIMATES,  TIME,  AND  OBSERVED  DATA 
%  NUMBER  OF  OBSERVATIONS  (input  from  marine. m) 
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%     ESTIMATED  TARGET  STATE 
xte  =  zeros(5 jkmax); 


%  initial  est.  target  state 


%     OBSERVER  (SENSOR)  INITIAL  POSITIONS  AND  VELOCITIES 
%  (input  from  marine,  ni) 
xfoa  =  zeros(4,kmax); 
xfoa(: ,1)  =  xfol(: ,1); 
xfol  =  xfoa; 

%  OBSERVER  SPEED  (input  from  marine,  m) 

%  INITIAL  TIME  SETTING 
Time  =  zeros( 1 ,kmax); 


%     ACTUAL  TRACK  POSITIONS  (input  from  marine. m) 
X  =  zeros( 1 jkmax); 
y  =  zeros( Ijkmax); 
T  =  0; 
for  k  =  1 

x(k)  =  tgt(l,l); 

y(k)  =  tgt(4,l); 

fo(k)  =tgt(7,l); 


end 
for 


k  = 


2: kmax 
T  =  T+dt; 
x(k)  =  (0. 


5'Vtgt(3,l)'V(T'.2))+(tgt(2,l).*T)+(tgt(l,l)); 


end 


vx(k)  =  (tgt(3,l)"T)+tgt(2,l); 

y(k)  =  (0.5^'^tgt(6,l)''^(TA2))+(tgt(5,l), 

vy(k)  =  (tgt(6,l)-'VT)+tgt(5,l); 

fo(k)  =  tgt(7,l); 


*T)+(tgt(4,l)); 


zpos  = 


X 

vx 

y 

vy 
fo 


%  rest  frequency  xmtr  (Hz) 


INPUT  OBSERVATION  VALUES  (BEARING/FREQUENCY) 
INITIAL  OBSERVED  TARGET  STATE 
xto  =  zeros(5 ,kmax); 


xto(: ,1)  =  [  tgt(l,l) 

tgt(2,l) 

tgt(4,l) 

tgt(5,l) 

tgt(7,l)  ]; 
deltlx  =  zpos(l,:)  -  xfol(l,: 
deltly  =  zpos(3,:)  -  xfol(3,: 
brgl  =  atan2(deltlx, deltly); 
vp  =  1.  0789e9; 


X  direction) 
in  X  direction) 
y  direction) 
in  y  direction) 


%  (km  in 
%  (km/hr 
%  (km  in 
%  (km/hr 
%   rest  frequency  xmtr  (Hz) 


bearing  thtal 
spd  light  (km/hr) 


for  k  =  1:  kmax 

f(k)   =  (zpos(5,k)^Wp)/(vp+((zpos(l,k)*zpos(2,k))+(zpos(3,k)*. 
zpos(4,k) ))/sqrt((zpos( l,k)A2+zpos(3,k)A2))); 
end 
zobs  =  [  brgl;  f]  ; 

xtol  =  zeros(5 jkraax); 
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xtoid, 

xtol(2, 
xtol(3, 
xtol(4, 
xtol(5. 


xtol(: ,1)  = 
xto  =  xtol; 


zpos( 1 , 
zpos( 2 , 
zpos(3 , 
zpos( 4 , 
zpos(5 , 
xto(: ,1); 


%  STATE  EXCITATION  AND  MEASUREMENT  ERROR  COVARIANCE  MATRICES 
varv  =  1.  005;  %  variance  of  linear  acceleration 

varth  =  5. 05;  %   variance  of  angular  acceleration 

vfreq  =1.0;  %  variance  of  the  rest  frequency 


R  =  [  le-11   0 

0   5e-ll  ] ; 


%   measurement  noise 


%   INPUT  OBSERVATION  VALUES  WITH  NOISE  ADDED 

rand( 'normal ' ) 

zobs  =  zobs  +  sqrt(R(  1 , 1)  )''''rand(zobs); 

%     INITIAL  KALMAN  MATRICES  AND  OBSERVATION 

I  =  eye(5"); 

%      INITIAL  KALMAN  P  MATRIX 

PO  =  [  300   0   0   0   0  %   reset  value  of  Error  Covariance 

0   300   0   0   0 

0   0   300   0   0 

0    0   0    300   0 

0    0   0     0   300   ]; 

%     REINITIALIZING  P  MATRIX 
P  =  PO; 


%  KALMAN  ESTIMATE 
xte(: ,1)  =  [  xto( 1,1) 
tgt(2,l) 
xto(3,l) 
tgt(5,l) 
tgt(7,l)  ]; 

tpred  =  xte( : , 1); 


%   (km  -  X  direction) 

%   (km/hr) 

/b  (km  -  y  direction) 

%   (km/hr) 

%  rest  frequency  xmtr  (Hz)_ 


%  SIMULATION  OF  KALMAN  FILTER  OPERATION 

%  NUMBER  OF  BAD  OBSERVATIONS 

nbad  =0;  %  initial  setting  bad  obs.  cntr, 

%  SIMULATION  COUNTER  LOOP 

k  =  0; 

for  kl=l:  10"kmax 

k  =  k  +  1;  %  actual  loop  counter 

if  (k  =  kmax)  %  check  for  termination 

break 
end 

%  UPDATE  FORWARD  OBSERVER'S/  FAC'S  POSITION  (IF  THEY  ARE  MOVING) 
if  k  <=  3 

way  =  pi/2; 
else 
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way  =  -(3'Vpi/4); 
end 
if  spd  ==  0 

xfol(l,k+l)  =  xfol(l,k); 

xfol(3,k+l)  =  xfol(3,k); 
else 

xfol(l,k+l)  =  xfol(l,k)  +  spd*sin(way)*dt; 

xfol(2,k+l)  =  spd''^sin(way); 

xfol(3,k+l)  =  xfol(3,k)  +  spd*cos(way)*dt; 

xfol(4,k+l)  =  spd''fcos(way); 
end 


%  PROJECT  AHEAD  KALMAN  STATE  ESTIMATE 
xte(: ,k+l)  =  phi*xte(: ,k); 

%  UPDATE  Q 

vt  =  sqrt((xte(2,k+l))A2  +  (xte(4,k+l) ) a2); 

sigxdd2  =  ((xte(2,k+l)/vt)A2'^varv)  +  ((xte(4,k+l)  A2)*varth); 

sigyQd2  =  ((xte(4,k+l)/vt)A2''fvarv)  +  ((xte(2  ,k+l)  A2)*varth); 
sigxydd  =  (xte(  2  ,k+l)-xte(4,k+l) )'''(  (varv/(  vt*vt)  )-varth); 

ql2 
ql3 
ql4 
ql5 
q21 
q:2 
q23 
q24 
q25 
q31 
q32 
q33 
q34 
q35 
q41 
q42 
q43 
q44 
q45 
qSl 
q52 
q53 
q54 
q55 


((dt--'-dt)/2)A2-'"^sigxdd2; 
((df-Mf-'--dt)/2)'^sigxdd2; 
((df-Mt)/2)'>sigxydd; 
(  (  dt " df-'-dt )  / 2 )  "S igxydd; 

0; 

ql2; 

(dt''-dt)*sigxdd2; 

( ( df-Mt "dt )  / 2 ) •■sigxydd; 

(df'^dt)  "sigxydd; 

0; 

ql3; 

q23; 

((dt'Mt)/2)A2*sigvdd2; 

( ( dt'Mt--Mt )  / 2  )--s  igydd2; 

0; 

ql4 

q24 

q34 

(dt 

0; 

ql5 
q25 
q35 
q45 
(dt^--dt) 


dt)"sigydd2; 


■'vfreq; 


Ql  =  [  qll  ql2  ql3  ql4  ql5 
q21  q22  q23  q24  q25 
q31  q32  q33  q34  q35 
q41  q42  q43  q44  q45 
q51  q52  q53  q54  q55  ]  ; 

Q  =  del^'Ql^(del'); 

%  PROJECT  ERROR  COVARIANCE 
P  =  phi--^P^--(phi')  +  Q; 
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%  UPDATE  H 

rl2  =  sqrt((xte(l,k+l)A2)+(xte(3,k+l)A2)); 

r32  =  (sqrt((xte(l,k+l)A2)+(xte(3,k+l)A2)))A3; 

deltxvy  =  xte(  1  ,k+l)'^xte(4,k+l)  -  xte(3,k+l)*xte(2,k+l); 

deltyvx  =  xte(3,k+l)*xte(2,k+l)  -  xte( l,k+l)*xte(4,k+l); 

fkvp  =  xte(5,k+l)*vp; 

hll  =  -(xte(3,k+l)/((xte(l,k+l)A  2)+(xte(3,k+l)A2))); 

hl2  =  0; 

hl3  =  xte(l,k+l)/((xte(l,k+l)A2)+(xte(3,k+l)A2)); 

hl4  =  0; 

hl5  =  0; 

h21  =  -((zobs(2,k+l)A2)/fkvp)*(xte(3,k+l)*deltyvx)/r32; 

h22  =  -((zobs(2,k+l)A2)/fkvp)*(xte(l,k+l)/rl2); 

h23  =  -(zobs(2,k+l)/fkvp)'>(xte(l,k+l)*deltxvy)/r32; 

h24  =  -((zobs(2,k+l)A2)/fkvp)*(xte(3,k+l)/rl2); 

h25  =  zobs(2,k+l)/xte(5,k+l); 

H  =  [   hll  hl2  hl3  hl4  hl5 

h21  h22  h23  h24  h25   ]; 

%  GET  NEW  MEASURED  ESTIMATE 

deltlx  =  xte(l,k+l)  -  xfol(l,k+l); 

deltly  =  xte(3,k+l)  -  xfol(3,k+l); 

zhatl  =  atan2(deltlx, deltly);  %   bearing  estimate  thtal 

zhat2  =  (xte(5,k+l)-''-vp)/(vp+((xte(l,k+l)*   %   frequency  estimate 

xte(2,k+l))+(xte(3,k+l)'^  .  . 

xte(4,k+l)))/sqrt((xte(l,k+l)A2+xte(3,k+l)A2))); 

zhat  =  [  zhatl 

zhat2  ] ; 

%  COMPUTATION  OF  KALMAN  GAIN 

vresid  =  (H"P"(H' )  +  R);  %  variance  of  the  residual 

G  =  P"(H')  /  vresid;  %  Kalman  gain 

%     COMPUTATION  OF  UPDATED  ERROR  COVARIANCE 
P  =  (I  -  G--'--H)^'-P; 

%  COMPUTATION  OF  RESIDUAL 
resid  =  zobs(: ,k+l)  -  zhat; 

BIG,  THEN  RESET  P  AND  DO  CALCULATIONS  AGAIN. 

0"sqrt(abs(vresid( 1 , 1)) )  ... 

>  1.  0-'-sqrt(abs(vresid(2,2)))  ... 


reset  P 


% 

IF 

RESIDUAL  IS  TOO 

if 

(abs(resid(l))  >  1 

]  abs(resid(2)) 

&  nbad  =  0) 

nbad  =  1; 

k  = 

=  k-1; 

P  = 

=  PO; 

el; 

se 

%  COMPUTATION  OF  UPDATED  ESTIMATE 
xte(: ,k+l)  =  xte(: ,k+l)  +  G-resid; 

%     ERROR  ELLIPSOID  PLOTTING  ROUTINE 
M  =  [xte(l,k+l)  xte(3,k+l)]; 
K  =  [  P(l,l)  P(l,3) 
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P(3,l)  P(3,3)  ]; 
[xp,yp]  =  errellip(M,K,.  60,25,0); 
eex(: ,k)  =  xp; 
eey(: ,k)  =  yp; 

%     PREDICTED  (FUTURE  TRACK)  USING  KALMAN  ESTIMATE 
tpred(: ,1)  =  xte(: ,k+l); 
ace  =  zeros(5 , 1); 
for  1  =  1:  kmax 
if  k  >=  3 

acc(2,l)  =  (xteC2,k+l)-xte(2,k))/dt; 
acc(4,l)  =  (xte(4,k+l)-xte(4,k))/dt; 
if  acc(2,l)  >=  5 
break 

ace  =  zeros(5 , 1); 
elseif  acc(4,l)  >=  5 
break 

ace  =  zeros(5,l); 
end 
end 

tpred(: ,1+1)  =  phi*tpred(: ,l)+del*acc; 
end 

nbad  =  0; 
end 
%     PLOT  OBSERVED  AND  ESTIMATED  TRACKS  (TARGET  AND  OBSERVER) 
plot(xfol(l,l:k+l),xfol(3,l:k+l),'xw'  ,xto(  1 , 1:  k+1) , 
xto(3,l:k+l)  ,  '.  b'  ,.  .  . 

xte(l,l:k+l),xte(3,l:k+l)  j'+w'  ,zpos(  1, 1:  k+1)  ,2pos(  3  , 1:  k+1) ,  'ow'  , 
tpred(l,:  )  ,tpred(3,:  )  ,  '-'-g  ,eex,eey,  '-r'  ),grid 
t it le( 'ACTUAL/OBSERVED/ESTIMATED/PREDICTED  TGT  TRKS 
USING  DOPPLER  '),... 

xlabeK 'DISTANCE  X' ) ,ylabel( 'DISTANCE  Y') 
end 
meta  doppler 


function    [x,y]  =  errellip(M,K,Pr,n,MD) 

O/ 

%  errellip(M,K,Pr,n,MD) 

% 

%  This  function  takes  the  following  arguments: 

% 

%  M     Mean  Vector  (2x1) 

%  K     Covariance  Matrix  (2  x  2) 

%  Pr    Probability  (0  . .  1) 

%  n     Number  of  points  to  compute 

%  MD    Compute  by  Mahalanobis  Distance  vice  probability 

%  0  =  Probability 

%  1  =  Mahalanobis  Distance 

% 

%  and  returns  x  and  y  vectors  of  the  confidence  ellipse  for  the 

%  given  probability  or  Mahalanobis  Distance. 

%  Stephen  L.  Spehn     15  Nov  1989 
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if  nargin  =  5 

error( ' Incorrect  number  of  arguments  to  errellip. '); 
end; 
if  MD  =  1 

c  =  abs(Pr); 
else 

Pr  =  max(min(. 995,Pr),. 005); 


%  Cubic  spline  fit  for  the  ellipse  constant. 
%     Using  this  method  is  a  compromise  between 


pp  =  [  12;  .  005;  .01; 
.95;  .975;  .99; 
4.  190809197869072e+l; 
3. 970929262330003; 

1.  62882241111034e+l; 

4.  1769857552230796+2; 
-3.  8103563280085248-1; 

5.  4440891692241426-1; 
2. 55841940780766; 
4. 60704229252476+2; 
2. 020857475864537; 

2.  122852230998054; 

3.  862381141104123; 

4.  3821332658986736+1; 
.01;  .0201;  .0506;  .103;  .211;  .575; 
1.39;  2.77;  4.61;  5.99;  7.38;  9.21  ]; 
c  =  ppval(pp,Pr); 


025;  .05;  .1;  .25;  .5; 

995;  4.0; 

4. 190809197872625e+l; 

2. 15930349191602; 

8.24375856514019e+l; 

2. 087990965023806e+5; 

2.475857468793011e-l; 

1. 140048306271903; 

1.4774587491135226+1; 

4. 920316224166436e+2; 

2. 02019022643493; 

2. 20707509215777; 

8.  195632865839841; 

6. 763972895071458e+l; 


speed  and  accuracy. 
.75;  .9; 

-2. 118721291999464e+l 
5.955793735647319e-l 
2.725551521454689e+3 
2. 087990965023842e+5 
2. 133449885921905; 
2. 111734877634124; 
5.  1871501034265896+1; 
9. 887990965023757e+3; 
2.055905760926949; 
2. 694842569743673; 
1.8192546144650046+1; 
2.233400677623216+2; 


end; 


%  Get  Eigenvectors,  Eigenvalues,  and  translated  variances 
[EveCjEval]  =  6ig(K); 
sigx  =  sqrt(Eval( 1 , 1)); 
sigy  =  sqrt(Eval(2,2)); 

7o     Parameterized  ellipse  equations 
t  =  0:(2'Vpi/n):(2'^pi); 
XV  =  sigx^'C"Cos(  t); 
yv  =  sigy-'^c-sinCt); 

%  Translate  back  to  the  original  coordinates 
X  =  (xv'>Evec(l,l)  +  yv''^Ev6c(l,2)  +  M(l))'; 
y  =  (xv^--Evec(2,l)  +  yv'-Evec(2 ,2)  +  M(2))'; 


function  y  =  reshape(x,m,n) 

%RESHAPE  RESHAPE(X,M,N)  returns  the  M-by-N  matrix  whose  elements 

%are  taken  columnwise  from  X.   An  error  results  if  X  does 

%not  have  M"''N  elements. 

mm,nn  =  size(x); 

if  mm-nn  =  m-n 

error( 'Matrix  must  have  M*N  elements.') 

end 
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y  =  2eros(m,n); 
y(:)    =  x; 
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